detector.cpp
Go to the documentation of this file.
00001 #include "checkerboard_pose_estimation/detector.h"
00002 #include "checkerboard_pose_estimation/cvcalibinit_lowres.h"
00003 
00004 #include <outlet_pose_estimation/detail/features.h> // ApplyGamma
00005 #include <limits>
00006 
00007 namespace checkerboard_pose_estimation {
00008 
00009 bool Detector::detect(const cv::Mat& image, std::vector<cv::Point2f>& points) const
00010 {
00011   IplImage ipl = (IplImage)image;
00012 
00013   // Do gamma correction
00014   ApplyGamma(&ipl, 1.5f); 
00015 
00016   // Find the checkerboard corners. First we try a specialized low-resolution detector,
00017   // falling back to the standard OpenCV detector.
00018   points.resize(width_ * height_);
00019   int corners_found = 0;
00020   int ret = cvFindChessboardCornersLowres(&ipl, cvSize(width_, height_),
00021                                           (CvPoint2D32f*)&points[0], &corners_found);
00022   if (!ret) {
00023     ret = cvFindChessboardCorners(&ipl, cvSize(width_, height_),
00024                                   (CvPoint2D32f*)&points[0], &corners_found,
00025                                   CV_CALIB_CB_ADAPTIVE_THRESH);
00026     if(!ret) {
00027       points.resize(corners_found);
00028       return false;
00029     }
00030   }
00031 
00032   // We enforce a unique ordering on the checkerboard corners. The lowres detector now
00033   // guarantees a unique ordering, but the fallback OpenCV detector does not yet.
00035   
00036   // Force consistent clockwise ordering on the four outside corners.
00037   cv::Point2f c0 = points[0];
00038   cv::Point2f c1 = points[width_ - 1];
00039   cv::Point2f c2 = points[(height_ - 1) * width_];
00040   if ((c1.x - c0.x)*(c2.y - c0.y) - (c2.x - c0.x)*(c1.y - c0.y) < 0) {
00041     for (int i = 0; i < height_; ++i)
00042       std::reverse(points.begin() + width_*i, points.begin() + width_*(i+1));
00043   }
00044 
00045   // Reverse the corners if the origin (corner 0) is on the wrong side.
00046   Side detected_side = (c0.x > c2.x) ? RIGHT : LEFT;
00047   if (detected_side != origin_side_)
00048     std::reverse(points.begin(), points.end());
00049 
00050   if (do_subpixel_refinement_) {
00051     // Make sure we use a conservative radius, less than the minimum distance between corners.
00052     // Otherwise subpixel "refinement" may move detected points to the wrong corners.
00053     float min_distance_sq = std::numeric_limits<float>::max();
00054     for (int row = 0; row < height_; ++row) {
00055       for (int col = 0; col < width_ - 1; ++col) {
00056         int index = row*width_ + col;
00057         cv::Point2f a = points[index], b = points[index+1];
00058         cv::Point2f diff = a - b;
00059         min_distance_sq = std::min(min_distance_sq, diff.x*diff.x + diff.y*diff.y);
00060       }
00061     }
00062     for (int row = 0; row < height_ - 1; ++row) {
00063       for (int col = 0; col < width_; ++col) {
00064         int index = row*width_ + col;
00065         cv::Point2f a = points[index], b = points[index+width_];
00066         cv::Point2f diff = a - b;
00067         min_distance_sq = std::min(min_distance_sq, diff.x*diff.x + diff.y*diff.y);
00068       }
00069     }
00070     int radius = std::sqrt(min_distance_sq) * 0.5f + 0.5f;
00071 
00072     cvFindCornerSubPix(&ipl, (CvPoint2D32f*)&points[0], corners_found,
00073                        cvSize(radius,radius), cvSize(-1,-1),
00074                        cvTermCriteria(CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1));
00075   }
00076 
00077   return true;
00078 }
00079 
00080 void Detector::getDisplayImage(const cv::Mat& source,
00081                                const std::vector<cv::Point2f>& points,
00082                                bool success, cv::Mat& display) const
00083 {
00084   static const int MAGNIFICATION = 4;
00085 
00087   cv::Mat color;
00088   cv::cvtColor(source, color, CV_GRAY2BGR);  
00089   cv::resize(color, display, cv::Size(), MAGNIFICATION, MAGNIFICATION);
00090 
00091   if (points.empty()) return;
00092   
00093   std::vector<cv::Point2f> scaled_points(points.size());
00094   for (size_t i = 0; i < points.size(); ++i)
00095     scaled_points[i] = cv::Point2f(points[i].x * MAGNIFICATION, points[i].y * MAGNIFICATION);
00096   cv::drawChessboardCorners(display, cv::Size(width_, height_), cv::Mat(scaled_points), success);
00097 }
00098 
00099 } //namespace checkerboard_pose_estimation


checkerboard_pose_estimation
Author(s): Patrick Mihelich
autogenerated on Thu Aug 27 2015 14:30:00