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
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);
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
00096
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