detect_calibration_pattern.cpp
Go to the documentation of this file.
00001 #include <turtlebot_actions/detect_calibration_pattern.h>
00002 
00003 void PatternDetector::setCameraMatrices(cv::Mat K_, cv::Mat D_)
00004 {
00005   K = K_;
00006   D = D_; 
00007 }
00008 
00009 void PatternDetector::setPattern(cv::Size grid_size_, float square_size_, 
00010       Pattern pattern_type_, cv::Point3f offset_)
00011 {
00012   ideal_points = calcChessboardCorners(grid_size_, square_size_, pattern_type_, offset_);
00013   pattern_type = pattern_type_;
00014   grid_size = grid_size_;
00015   square_size = square_size_;
00016 }
00017 
00018 object_pts_t PatternDetector::calcChessboardCorners(cv::Size boardSize,
00019                                           float squareSize,
00020                                           Pattern patternType,
00021                                           cv::Point3f offset)
00022 {
00023   object_pts_t corners;
00024   switch (patternType)
00025   {
00026     case CHESSBOARD:
00027     case CIRCLES_GRID:
00028       for (int i = 0; i < boardSize.height; i++)
00029         for (int j = 0; j < boardSize.width; j++)
00030           corners.push_back(
00031                             cv::Point3f(float(j * squareSize),
00032                                         float(i * squareSize), 0) + offset);
00033       break;
00034     case ASYMMETRIC_CIRCLES_GRID:
00035       for (int i = 0; i < boardSize.height; i++)
00036         for (int j = 0; j < boardSize.width; j++)
00037           corners.push_back(
00038                             cv::Point3f(float(i * squareSize),
00039                                         float((2 * j + i % 2) * squareSize), 0) + offset);
00040       break;
00041     default:
00042       std::logic_error("Unknown pattern type.");
00043   }
00044   return corners;
00045 }
00046 
00047 
00048 
00049 int PatternDetector::detectPattern(cv::Mat& inm, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation)
00050 {
00051   translation.setZero();
00052   orientation.setIdentity();
00053   
00054   bool found = false;
00055   
00056   observation_pts_t observation_points;
00057   
00058   switch (pattern_type)
00059   {
00060     case ASYMMETRIC_CIRCLES_GRID:
00061       found = cv::findCirclesGrid(inm, grid_size, observation_points,
00062                                 cv::CALIB_CB_ASYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING);
00063       break;
00064     case CHESSBOARD:
00065       found = cv::findChessboardCorners(inm, grid_size, observation_points, cv::CALIB_CB_ADAPTIVE_THRESH);
00066       break;
00067     case CIRCLES_GRID:
00068       found = cv::findCirclesGrid(inm, grid_size, observation_points, cv::CALIB_CB_SYMMETRIC_GRID);
00069       break;
00070   }
00071 
00072   if(found)
00073   {
00074     // Do subpixel ONLY IF THE PATTERN IS A CHESSBOARD
00075     if (pattern_type == CHESSBOARD)
00076     {
00077       cv::cornerSubPix(inm, observation_points, cv::Size(5,5), cv::Size(-1,-1), 
00078       cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 100, 0.01));
00079     }
00080   
00081     cv::solvePnP(cv::Mat(ideal_points), cv::Mat(observation_points), K, D,
00082                  rvec, tvec, false);
00083     cv::Rodrigues(rvec, R); //take the 3x1 rotation representation to a 3x3 rotation matrix.
00084     
00085     cv::drawChessboardCorners(inm, grid_size, cv::Mat(observation_points), found);
00086     
00087     convertCVtoEigen(tvec, R, translation, orientation);
00088   }
00089   
00090   return found;
00091 }
00092 
00093 void convertCVtoEigen(cv::Mat& tvec, cv::Mat& R, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation)
00094 {
00095   // This assumes that cv::Mats are stored as doubles. Is there a way to check this?
00096   // Since it's templated...
00097   translation = Eigen::Vector3f(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0, 2));
00098   
00099   Eigen::Matrix3f Rmat;
00100   Rmat << R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
00101           R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
00102           R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2);
00103                                           
00104   orientation = Eigen::Quaternionf(Rmat);
00105 
00106 }
00107 


turtlebot_actions
Author(s): Helen Oleynikova, Melonee Wise
autogenerated on Wed Aug 26 2015 16:34:28