123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207 |
- #include "YoloDetector.h"
- #include <glog/logging.h>
- void YoloDetector::free_img(image_t m)
- {
- if (m.data)
- {
- free(m.data);
- }
- }
- Error_manager YoloDetector::set_region(float minx, float maxx, float miny, float maxy,float freq)
- {
- if(m_maxx<=m_minx || m_maxy<=m_miny || m_freq<=0)
- {
- char description[255]={0};
- sprintf(description,"set yolo parameter l:%.3f r:%.3f t:%.3f b:%.3f",minx,maxx,miny,maxy);
- return Error_manager(LOCATER_YOLO_PARAMETER_INVALID,NORMAL,description);
- }
- m_minx = minx;
- m_maxx = maxx;
- m_miny = miny;
- m_maxy = maxy;
- m_freq = freq;
- return SUCCESS;
- }
- YoloDetector::YoloDetector(std::string cfg, std::string weights, float minx, float maxx, float miny, float maxy,float freq)
- {
- #if YOLO_API_USED
- m_pDetector = new Detector(cfg, weights);
- m_minx = minx;
- m_maxx = maxx;
- m_miny = miny;
- m_maxy = maxy;
- m_freq = freq;
- //LOG(INFO) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
- #endif
- }
- YoloDetector::~YoloDetector()
- {
- #if YOLO_API_USED
- if (m_pDetector)
- {
- delete m_pDetector;
- m_pDetector = NULL;
- }
- #endif
- }
- Error_manager YoloDetector::detect(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, std::vector<YoloBox>& boxes, std::string save_dir)
- {
- boxes.clear();
- #if YOLO_API_USED
- /*int w = int((m_maxx - m_minx) / m_freq);
- int h = int((m_maxy - m_miny) / m_freq);*/
- int w = 416;
- int h = 208;
-
- cv::Mat img = cv::Mat::zeros(cv::Size(w, h * 2), CV_8UC1);
- for (int i = 0; i < cloud_in->size(); ++i)
- {
- pcl::PointXYZ point = cloud_in->points[i];
- float x = point.x;
- float y = point.y;
- float z = point.z;
- if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.&&z>50.)
- {
- int c = int((x - m_minx) / (m_maxx-m_minx)*w);
- int r = int((y - m_miny) / (m_maxy-m_miny)*h);
- int gray = int(z / 2000. * 256);
- if (gray > img.at<uchar>(r, c))
- img.at<uchar>(r, c) = gray;
- }
- }
- if (m_pDetector)
- {
- boxes.clear();
- cv::cvtColor(img,img,CV_GRAY2RGB);
- std::shared_ptr<image_t> img_t=mat_to_image_resize(img);
- std::vector<bbox_t> rects=m_pDetector->detect(*img_t, 0.5);
- //free_img(*img_t);
- cv::Mat rgb=img.clone();
- float ext = 0.0;
-
- for (int i = 0; i < rects.size(); ++i)
- {
- /*bbox_t box = rects[i];
- bbox_t box_t = box;
- float len = std::min(box.w*m_freq, box.h*m_freq);
- box_t.x = int(box.x*m_freq + m_minx - ext*len);
- box_t.y = int(box.y*m_freq + m_miny - ext*len);
- box_t.w = int(box.w*m_freq*(1.0+2*ext));
- box_t.h = int(box.h*m_freq*(1.0+2*ext));*/
- bbox_t box = rects[i];
- YoloBox box_y;
- box_y.x = (float(box.x) / float(w)*(m_maxx - m_minx) + m_minx);
- box_y.y = (float(box.y) /float(h)*(m_maxy - m_miny) + m_miny);
- box_y.w = (float(box.w)/float(w)*(m_maxx - m_minx));
- box_y.h = (float(box.h)/float(h)*(m_maxy - m_miny));
- //LOG(WARNING) << "\tyolo minx:" << m_minx << ",maxx:" << m_maxx << ",miny:" << m_miny << ",maxy:" << m_maxy;
- //LOG(WARNING) << "box " << i << " x:" << box.x << " ,y:" << box.y << " ,w:" << box.w << " ,h:" << box.h;
- //LOG(INFO) << "yolo box " << i << " x:" << box_y.x << " ,y:" << box_y.y << " ,w:" << box_y.w << " ,h:" << box_y.h;
- boxes.push_back(box_y);
- //����boxes
- cv::rectangle(rgb, cv::Point(box.x, box.y),
- cv::Point(box.x + box.w, box.y + box.h), cv::Scalar(0, 255, 0), 1);
- }
- char num[32] = { 0 };
- sprintf(num, " size=%d", rects.size());
- cv::putText(rgb, num, cv::Point(50, 50), 1, 1, cv::Scalar(0, 255, 0));
- cv::imwrite(save_dir + "/yolo.jpg", rgb);
- if (boxes.size() > 0)
- return SUCCESS;
- else
- return Error_manager(LOCATER_YOLO_DETECT_NO_TARGET,NORMAL,"yolo detect no car");
- }
- #endif
- return Error_manager(LOCATER_YOLO_DETECT_FAILED,NORMAL,"YOLO detect failed");
- }
- Error_manager YoloDetector::detect(cv::Mat& l, cv::Mat& r, std::vector<YoloBox>& boxes,std::string save_dir)
- {
- #if YOLO_API_USED
- int w = int((m_maxx - m_minx) / m_freq);
- int h = int((m_maxy - m_miny) / m_freq);
- cv::Mat img = cv::Mat::zeros(cv::Size(w, h*2), CV_8UC1);
- for (int i = 0; i < l.rows; ++i)
- {
- for (int j = 0; j < l.cols; ++j)
- {
- cv::Vec3f vec_l = l.at<cv::Vec3f>(i, j);
- cv::Vec3f vec_r = r.at<cv::Vec3f>(i, j);
-
- float x = vec_l[0];
- float y = vec_l[1];
- float z = vec_l[2];
- if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.)
- {
- int c = int((x - m_minx) / m_freq);
- int r = int((y - m_miny) / m_freq);
- int gray = int(z / 2000. * 256);
- if (gray > img.at<uchar>(r, c))
- img.at<uchar>(r, c) = gray;
- }
- x = vec_r[0];
- y = vec_r[1];
- z = vec_r[2];
- if (x >= m_minx&&x < m_maxx&&y >= m_miny&&y < m_maxy&&z < 2000.)
- {
- int c = int((x - m_minx) / m_freq);
- int r = int((y - m_miny) / m_freq);
- int gray = int(z / 2000. * 256);
- if (gray > img.at<uchar>(r, c))
- img.at<uchar>(r, c) = gray;
- }
- }
- }
- if (m_pDetector)
- {
- boxes.clear();
- std::shared_ptr<image_t> img_t=mat_to_image_resize(img);
- std::vector<bbox_t> rects=m_pDetector->detect(*img_t, 0.5);
- free_img(*img_t);
- cv::Mat rgb;
- cv::cvtColor(img, rgb, CV_GRAY2RGB);
- for (int i = 0; i < rects.size(); ++i)
- {
- bbox_t box = rects[i];
- YoloBox box_t;
- box_t.x = (box.x*m_freq+m_minx);
- box_t.y = (box.y*m_freq+m_miny);
- box_t.w = (box.w*m_freq);
- box_t.h = (box.h*m_freq);
- boxes.push_back(box_t);
- //����boxes
-
- cv::rectangle(rgb, cv::Point(box.x, box.y ),
- cv::Point(box.x+box.w, box.y + box.h), cv::Scalar(0, 255, 0), 1);
-
- }
- char num[32] = { 0 };
- sprintf(num, " size=%d", rects.size());
- cv::putText(rgb, num, cv::Point(50, 50), 1, 1, cv::Scalar(0, 255, 0));
- cv::imwrite(save_dir + "/yolo.jpg", rgb(cv::Rect(0,0,rgb.cols,rgb.rows/2)));
- return SUCCESS;
- }
- #endif
- return Error_manager(LOCATER_YOLO_DETECT_FAILED,NORMAL,"yolo api not define,(no code)");
- }
|