detect_calibration_pattern.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <turtlebot_kinect_arm_calibration/detect_calibration_pattern.h>
00031 
00032 void PatternDetector::setCameraMatrices(cv::Mat K_, cv::Mat D_)
00033 {
00034   K = K_;
00035   D = D_; 
00036 }
00037 
00038 void PatternDetector::setPattern(cv::Size grid_size_, float square_size_, 
00039       Pattern pattern_type_, cv::Point3f offset_)
00040 {
00041   ideal_points = calcChessboardCorners(grid_size_, square_size_, pattern_type_, offset_);
00042   pattern_type = pattern_type_;
00043   grid_size = grid_size_;
00044   square_size = square_size_;
00045 }
00046 
00047 object_pts_t PatternDetector::calcChessboardCorners(cv::Size boardSize,
00048                                           float squareSize,
00049                                           Pattern patternType,
00050                                           cv::Point3f offset)
00051 {
00052   object_pts_t corners;
00053   switch (patternType)
00054   {
00055     case CHESSBOARD:
00056     case CIRCLES_GRID:
00057       for (int i = 0; i < boardSize.height; i++)
00058         for (int j = 0; j < boardSize.width; j++)
00059           corners.push_back(
00060                             cv::Point3f(float(j * squareSize),
00061                                         float(i * squareSize), 0) + offset);
00062       break;
00063     case ASYMMETRIC_CIRCLES_GRID:
00064       for (int i = 0; i < boardSize.height; i++)
00065         for (int j = 0; j < boardSize.width; j++)
00066           corners.push_back(
00067                             cv::Point3f(float(i * squareSize),
00068                                         float((2 * j + i % 2) * squareSize), 0) + offset);
00069       break;
00070     default:
00071       std::logic_error("Unknown pattern type.");
00072   }
00073   return corners;
00074 }
00075 
00076 
00077 
00078 int PatternDetector::detectPattern(cv::Mat& image_in, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation, cv::Mat& image_out)
00079 {
00080   translation.setZero();
00081   orientation.setIdentity();
00082   
00083   bool found = false;
00084   
00085   observation_pts_t observation_points;
00086   
00087   switch (pattern_type)
00088   {
00089     case ASYMMETRIC_CIRCLES_GRID:
00090       found = cv::findCirclesGrid(image_in, grid_size, observation_points,
00091                                 cv::CALIB_CB_ASYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING);
00092       break;
00093     case CHESSBOARD:
00094       found = cv::findChessboardCorners(image_in, grid_size, observation_points, cv::CALIB_CB_ADAPTIVE_THRESH);
00095       break;
00096     case CIRCLES_GRID:
00097       found = cv::findCirclesGrid(image_in, grid_size, observation_points, cv::CALIB_CB_SYMMETRIC_GRID);
00098       break;
00099   }
00100 
00101   if(found)
00102   {
00103     // Do subpixel ONLY IF THE PATTERN IS A CHESSBOARD
00104     if (pattern_type == CHESSBOARD)
00105     {
00106       cv::cornerSubPix(image_in, observation_points, cv::Size(5,5), cv::Size(-1,-1), 
00107       cv::TermCriteria(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 100, 0.01));
00108     }
00109   
00110     cv::solvePnP(cv::Mat(ideal_points), cv::Mat(observation_points), K, D,
00111                  rvec, tvec, false);
00112     cv::Rodrigues(rvec, R); //take the 3x1 rotation representation to a 3x3 rotation matrix.
00113     
00114     cv::drawChessboardCorners(image_out, grid_size, cv::Mat(observation_points), found);
00115     
00116     convertCVtoEigen(tvec, R, translation, orientation);
00117   }
00118   
00119   return found;
00120 }
00121 
00122 void convertCVtoEigen(cv::Mat& tvec, cv::Mat& R, Eigen::Vector3f& translation, Eigen::Quaternionf& orientation)
00123 {
00124   // This assumes that cv::Mats are stored as doubles. Is there a way to check this?
00125   // Since it's templated...
00126   translation = Eigen::Vector3f(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0, 2));
00127   
00128   Eigen::Matrix3f Rmat;
00129   Rmat << R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2),
00130           R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2),
00131           R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2);
00132                                           
00133   orientation = Eigen::Quaternionf(Rmat);
00134 
00135 }
00136 


turtlebot_kinect_arm_calibration
Author(s): Helen Oleynikova
autogenerated on Thu Dec 12 2013 12:33:33