blob_detector.cpp
Go to the documentation of this file.
00001 #include <costmap_converter/costmap_to_dynamic_obstacles/blob_detector.h>
00002 #include <opencv2/opencv.hpp>
00003 #include <iostream>
00004 
00005 BlobDetector::BlobDetector(const SimpleBlobDetector::Params& parameters) : params_(parameters) {}
00006 
00007 cv::Ptr<BlobDetector> BlobDetector::create(const cv::SimpleBlobDetector::Params& params)
00008 {
00009   return cv::Ptr<BlobDetector> (new BlobDetector(params)); // compatibility with older versions
00010   //return cv::makePtr<BlobDetector>(params);
00011 }
00012 
00013 void BlobDetector::detect(const cv::Mat& image, std::vector<cv::KeyPoint>& keypoints, const cv::Mat&)
00014 {
00015   // TODO: support mask
00016   contours_.clear();
00017 
00018   keypoints.clear();
00019   cv::Mat grayscale_image;
00020   if (image.channels() == 3)
00021     cv::cvtColor(image, grayscale_image, CV_BGR2GRAY);
00022   else
00023     grayscale_image = image;
00024 
00025   if (grayscale_image.type() != CV_8UC1)
00026   {
00027     //CV_Error(cv::Error::StsUnsupportedFormat, "Blob detector only supports 8-bit images!");
00028     std::cerr << "Blob detector only supports 8-bit images!\n";
00029   }
00030 
00031   std::vector<std::vector<Center>> centers;
00032   std::vector<std::vector<cv::Point>> contours;
00033   for (double thresh = params_.minThreshold; thresh < params_.maxThreshold; thresh += params_.thresholdStep)
00034   {
00035     cv::Mat binarized_image;
00036     cv::threshold(grayscale_image, binarized_image, thresh, 255, cv::THRESH_BINARY);
00037 
00038     std::vector<Center> cur_centers;
00039     std::vector<std::vector<cv::Point>> cur_contours, new_contours;
00040     findBlobs(grayscale_image, binarized_image, cur_centers, cur_contours);
00041     std::vector<std::vector<Center>> new_centers;
00042     for (std::size_t i = 0; i < cur_centers.size(); ++i)
00043     {
00044       bool isNew = true;
00045       for (std::size_t j = 0; j < centers.size(); ++j)
00046       {
00047         double dist = cv::norm(centers[j][centers[j].size() / 2].location - cur_centers[i].location);
00048         isNew = dist >= params_.minDistBetweenBlobs && dist >= centers[j][centers[j].size() / 2].radius &&
00049                 dist >= cur_centers[i].radius;
00050         if (!isNew)
00051         {
00052           centers[j].push_back(cur_centers[i]);
00053 
00054           size_t k = centers[j].size() - 1;
00055           while (k > 0 && centers[j][k].radius < centers[j][k - 1].radius)
00056           {
00057             centers[j][k] = centers[j][k - 1];
00058             k--;
00059           }
00060           centers[j][k] = cur_centers[i];
00061 
00062           break;
00063         }
00064       }
00065       if (isNew)
00066       {
00067         new_centers.push_back(std::vector<Center>(1, cur_centers[i]));
00068         new_contours.push_back(cur_contours[i]);
00069       }
00070     }
00071     std::copy(new_centers.begin(), new_centers.end(), std::back_inserter(centers));
00072     std::copy(new_contours.begin(), new_contours.end(), std::back_inserter(contours));
00073   }
00074 
00075   for (size_t i = 0; i < centers.size(); ++i)
00076   {
00077     if (centers[i].size() < params_.minRepeatability)
00078       continue;
00079     cv::Point2d sum_point(0, 0);
00080     double normalizer = 0;
00081     for (std::size_t j = 0; j < centers[i].size(); ++j)
00082     {
00083       sum_point += centers[i][j].confidence * centers[i][j].location;
00084       normalizer += centers[i][j].confidence;
00085     }
00086     sum_point *= (1. / normalizer);
00087     cv::KeyPoint kpt(sum_point, (float)(centers[i][centers[i].size() / 2].radius));
00088     keypoints.push_back(kpt);
00089     contours_.push_back(contours[i]);
00090   }
00091 }
00092 
00093 void BlobDetector::findBlobs(const cv::Mat& image, const cv::Mat& binary_image, std::vector<Center>& centers,
00094                              std::vector<std::vector<cv::Point>>& cur_contours) const
00095 {
00096   (void)image;
00097   centers.clear();
00098   cur_contours.clear();
00099 
00100   std::vector<std::vector<cv::Point>> contours;
00101   cv::Mat tmp_binary_image = binary_image.clone();
00102   cv::findContours(tmp_binary_image, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
00103 
00104   for (std::size_t contour_idx = 0; contour_idx < contours.size(); ++contour_idx)
00105   {
00106     Center center;
00107     center.confidence = 1;
00108     cv::Moments moms = cv::moments(cv::Mat(contours[contour_idx]));
00109     if (params_.filterByArea)
00110     {
00111       double area = moms.m00;
00112       if (area < params_.minArea || area >= params_.maxArea)
00113         continue;
00114     }
00115 
00116     if (params_.filterByCircularity)
00117     {
00118       double area = moms.m00;
00119       double perimeter = cv::arcLength(cv::Mat(contours[contour_idx]), true);
00120       double ratio = 4 * CV_PI * area / (perimeter * perimeter);
00121       if (ratio < params_.minCircularity || ratio >= params_.maxCircularity)
00122         continue;
00123     }
00124 
00125     if (params_.filterByInertia)
00126     {
00127       double denominator = std::sqrt(std::pow(2 * moms.mu11, 2) + std::pow(moms.mu20 - moms.mu02, 2));
00128       const double eps = 1e-2;
00129       double ratio;
00130       if (denominator > eps)
00131       {
00132         double cosmin = (moms.mu20 - moms.mu02) / denominator;
00133         double sinmin = 2 * moms.mu11 / denominator;
00134         double cosmax = -cosmin;
00135         double sinmax = -sinmin;
00136 
00137         double imin = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmin - moms.mu11 * sinmin;
00138         double imax = 0.5 * (moms.mu20 + moms.mu02) - 0.5 * (moms.mu20 - moms.mu02) * cosmax - moms.mu11 * sinmax;
00139         ratio = imin / imax;
00140       }
00141       else
00142       {
00143         ratio = 1;
00144       }
00145 
00146       if (ratio < params_.minInertiaRatio || ratio >= params_.maxInertiaRatio)
00147         continue;
00148 
00149       center.confidence = ratio * ratio;
00150     }
00151 
00152     if (params_.filterByConvexity)
00153     {
00154       std::vector<cv::Point> hull;
00155       cv::convexHull(cv::Mat(contours[contour_idx]), hull);
00156       double area = cv::contourArea(cv::Mat(contours[contour_idx]));
00157       double hullArea = cv::contourArea(cv::Mat(hull));
00158       double ratio = area / hullArea;
00159       if (ratio < params_.minConvexity || ratio >= params_.maxConvexity)
00160         continue;
00161     }
00162 
00163     if (moms.m00 == 0.0)
00164       continue;
00165     center.location = cv::Point2d(moms.m10 / moms.m00, moms.m01 / moms.m00);
00166 
00167     if (params_.filterByColor)
00168     {
00169       if (binary_image.at<uchar>(cvRound(center.location.y), cvRound(center.location.x)) != params_.blobColor)
00170         continue;
00171     }
00172 
00173     // compute blob radius
00174     {
00175       std::vector<double> dists;
00176       for (std::size_t point_idx = 0; point_idx < contours[contour_idx].size(); ++point_idx)
00177       {
00178         cv::Point2d pt = contours[contour_idx][point_idx];
00179         dists.push_back(cv::norm(center.location - pt));
00180       }
00181       std::sort(dists.begin(), dists.end());
00182       center.radius = (dists[(dists.size() - 1) / 2] + dists[dists.size() / 2]) / 2.;
00183     }
00184 
00185     centers.push_back(center);
00186     cur_contours.push_back(contours[contour_idx]);
00187   }
00188 }
00189 
00190 void BlobDetector::updateParameters(const Params& parameters)
00191 {
00192   params_ = parameters;
00193 }


costmap_converter
Author(s): Christoph Rösmann , Franz Albers , Otniel Rinaldo
autogenerated on Sat Jun 8 2019 20:53:15