checkerboard_detector.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; -*-
00002 // Software License Agreement (BSD License)
00003 // Copyright (c) 2008, JSK Lab, Inc.
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * The name of the author may not be used to endorse or promote products
00012 //     derived from this software without specific prior written permission.
00013 //
00014 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00015 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00016 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00017 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00018 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00019 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00020 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00021 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00022 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00023 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00024 // POSSIBILITY OF SUCH DAMAGE.
00025 //
00026 // author: Rosen Diankov
00027 #include <algorithm>
00028 
00029 #include <cstdio>
00030 #include <vector>
00031 #include <sstream>
00032 #include <algorithm>
00033 #include <ros/ros.h>
00034 
00035 #include <boost/thread/mutex.hpp>
00036 
00037 #include "opencv2/opencv.hpp"
00038 #include "cv_bridge/cv_bridge.h"
00039 #include "sensor_msgs/image_encodings.h"
00040 #include "sensor_msgs/CameraInfo.h"
00041 #include "sensor_msgs/Image.h"
00042 #include "image_geometry/pinhole_camera_model.h"
00043 #include "posedetection_msgs/ObjectDetection.h"
00044 #include "posedetection_msgs/Detect.h"
00045 #include "geometry_msgs/PointStamped.h"
00046 #include "geometry_msgs/PoseStamped.h"
00047 #include "math.h"
00048 #include "geometry_msgs/PolygonStamped.h"
00049 #include "jsk_recognition_msgs/PolygonArray.h"
00050 #include "eigen_conversions/eigen_msg.h"
00051 #include <sys/timeb.h>    // ftime(), struct timeb
00052 #include <sys/time.h>
00053 #include <dynamic_reconfigure/server.h>
00054 #include <checkerboard_detector/CheckerboardDetectorConfig.h>
00055 
00056 using namespace std;
00057 using namespace ros;
00058 
00059 class CheckerboardDetector
00060 {
00061     template <typename T>
00062     static vector<T> tokenizevector(const string& s)
00063     {
00064         stringstream ss(s);
00065         return vector<T>((istream_iterator<T>(ss)), istream_iterator<T>());
00066     }
00067 
00068 public:
00069     struct CHECKERBOARD
00070     {
00071         CvSize griddims; 
00072         vector<cv::Point3f> grid3d;
00073         //vector<CvPoint2D32f> corners;
00074         //cv::Mat corners;
00075         vector<cv::Point2f> corners;
00076         TransformMatrix tlocaltrans;
00077         std::string board_type;
00078     };
00079 
00080     posedetection_msgs::ObjectDetection _objdetmsg;
00081     sensor_msgs::CameraInfo _camInfoMsg;
00082 
00083     ros::Subscriber camInfoSubscriber,camInfoSubscriber2;
00084     ros::Subscriber imageSubscriber,imageSubscriber2;
00085     ros::Publisher _pubDetection;
00086     ros::Publisher _pubPoseStamped;
00087     ros::Publisher _pubCornerPoint;
00088     ros::Publisher _pubPolygonArray;
00089     ros::ServiceServer _srvDetect;
00090     int message_throttle_;
00091     int message_throttle_counter_;
00092     string frame_id; // tf frame id
00093     bool invert_color;
00094     int display, verbose, maxboard, queue_size, publish_queue_size;
00095     vector<CHECKERBOARD> vcheckers; // grid points for every checkerboard
00096     vector< string > vstrtypes; // type names for every grid point
00097     map<string,int> maptypes;
00098     ros::Time lasttime;
00099     boost::mutex mutex;
00100     ros::NodeHandle _node;
00101     int dimx, dimy;
00102     bool use_P;
00103     double fRectSize[2];
00104     typedef checkerboard_detector::CheckerboardDetectorConfig Config;
00105     boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv;
00106     bool adaptive_thresh_flag;
00107     bool filter_quads_flag;
00108     bool normalize_image_flag;
00109     bool fast_check_flag;
00110     double axis_size_;
00111     int circle_size_;
00112 
00114     // Constructor
00115     CheckerboardDetector()
00116     {
00117         _node.param("display", display, 0);
00118         _node.param("verbose", verbose, 1);
00119         _node.param("maxboard", maxboard, -1);
00120         _node.param("invert_color", invert_color, false);
00121         _node.param("use_P", use_P, false);
00122         _node.param("message_throttle", message_throttle_, 1);
00123         _node.param("queue_size", queue_size, 1);
00124         _node.param("publish_queue_size", publish_queue_size, 1);
00125         _node.param("axis_size", axis_size_, 0.05);
00126         _node.param("circle_size", circle_size_, 6);
00127 
00128         char str[32];
00129         int index = 0;
00130 
00131         srv = boost::make_shared <dynamic_reconfigure::Server<Config> > (_node);
00132         typename dynamic_reconfigure::Server<Config>::CallbackType f =
00133             boost::bind (&CheckerboardDetector::configCallback, this, _1, _2);
00134         srv->setCallback (f);
00135 
00136         while(1) {
00137             string type;
00138 
00139             sprintf(str,"grid%d_size_x",index);
00140             if( !_node.getParam(str,dimx) )
00141                 break;
00142             if (dimx < 3) {
00143                 ROS_ERROR("Param: %s must be greater than 2",str);
00144                 return;
00145             }
00146 
00147             sprintf(str,"grid%d_size_y",index);
00148             if( !_node.getParam(str,dimy) )
00149                 break;
00150             if (dimy < 3) {
00151                 ROS_ERROR("Param: %s must be greater than 2",str);
00152                 return;
00153             }
00154 
00155             sprintf(str,"rect%d_size_x",index);
00156             if( !_node.getParam(str,fRectSize[0]) )
00157                 break;
00158 
00159             sprintf(str,"rect%d_size_y",index);
00160             if( !_node.getParam(str,fRectSize[1]) )
00161                 break;
00162 
00163             sprintf(str,"type%d",index);
00164             if( !_node.getParam(str,type) ) {
00165                 sprintf(str,"checker%dx%d", dimx, dimy);
00166                 type = str;
00167             }
00168 
00169             std::string board_type;
00170             _node.param("board_type", board_type, std::string("chess"));
00171             
00172             
00173             string strtranslation,strrotation;
00174             sprintf(str,"translation%d",index);
00175             _node.param(str,strtranslation,string());
00176 
00177             vector<float> vtranslation = tokenizevector<float>(strtranslation);
00178             sprintf(str,"rotation%d",index);
00179             _node.param(str,strrotation,string());
00180 
00181             vector<float> vrotation = tokenizevector<float>(strrotation);
00182 
00183             CHECKERBOARD cb;
00184             cb.griddims = cvSize(dimx,dimy);
00185             cb.board_type = board_type;
00186             cb.grid3d.resize(dimx*dimy);
00187             int j=0;
00188             if (board_type == "chess" || board_type == "circle" || board_type == "circles") {
00189               for(int y=0; y<dimy; ++y)
00190                 for(int x=0; x<dimx; ++x)
00191                   cb.grid3d[j++] = cv::Point3f(x*fRectSize[0], y*fRectSize[1], 0);
00192             }
00193             else if (board_type == "acircle" || board_type == "acircles") {
00194               for(int ii=0; ii<dimy; ii++) {
00195                 for(int jj=0; jj<dimx; jj++) {
00196                   cb.grid3d[j++] = cv::Point3f((2*jj + ii % 2)*fRectSize[0],
00197                                           ii*fRectSize[1],
00198                                           0);
00199                 }
00200               }
00201             }
00202 
00203             if( vtranslation.size() == 3 )
00204                 cb.tlocaltrans.trans = 
00205                     Vector(vtranslation[0],vtranslation[1],vtranslation[2]);
00206 
00207             if( vrotation.size() == 9 )  {
00208                 for(int k = 0; k < 3; ++k) {
00209                     cb.tlocaltrans.m[4*k+0] = vrotation[3*k+0];
00210                     cb.tlocaltrans.m[4*k+1] = vrotation[3*k+1];
00211                     cb.tlocaltrans.m[4*k+2] = vrotation[3*k+2];
00212                 }
00213             }
00214 
00215             vcheckers.push_back(cb);
00216             vstrtypes.push_back(type);
00217             maptypes[vstrtypes.back()] = index;
00218             index++;
00219         }
00220 
00221         _node.param("frame_id", frame_id,string(""));
00222 
00223         if( maptypes.size() == 0 ) {
00224             ROS_ERROR("no checkerboards to detect");
00225             return;
00226         }
00227 
00228         if( display ) {
00229           cv::namedWindow("Checkerboard Detector",
00230                           (display == 1? CV_WINDOW_NORMAL : display));
00231         }
00232 
00233         lasttime = ros::Time::now();
00234         if (!display) {
00235             ros::SubscriberStatusCallback connect_cb = boost::bind( &CheckerboardDetector::connectCb, this);
00236             _pubDetection =
00237                 _node.advertise<posedetection_msgs::ObjectDetection> ("ObjectDetection", publish_queue_size,
00238                                                                       connect_cb, connect_cb);
00239             _pubPoseStamped =
00240                 _node.advertise<geometry_msgs::PoseStamped> ("objectdetection_pose", publish_queue_size,
00241                                                              connect_cb, connect_cb);
00242             _pubCornerPoint = _node.advertise<geometry_msgs::PointStamped>("corner_point", publish_queue_size, connect_cb, connect_cb);
00243             _pubPolygonArray = _node.advertise<jsk_recognition_msgs::PolygonArray>("polygons", publish_queue_size, connect_cb, connect_cb);
00244         }
00245         else {
00246             _pubDetection =
00247                 _node.advertise<posedetection_msgs::ObjectDetection> ("ObjectDetection", publish_queue_size);
00248             _pubPoseStamped =
00249                 _node.advertise<geometry_msgs::PoseStamped> ("objectdetection_pose", publish_queue_size);
00250             _pubCornerPoint = _node.advertise<geometry_msgs::PointStamped>("corner_point", publish_queue_size);
00251             _pubPolygonArray = _node.advertise<jsk_recognition_msgs::PolygonArray>("polygons", publish_queue_size);
00252             subscribe();
00253         }
00254         //this->camInfoSubscriber = _node.subscribe("camera_info", 1, &CheckerboardDetector::caminfo_cb, this);
00255         //this->imageSubscriber = _node.subscribe("image",1, &CheckerboardDetector::image_cb, this);
00256         //this->camInfoSubscriber2 = _node.subscribe("CameraInfo", 1, &CheckerboardDetector::caminfo_cb2, this);
00257         //this->imageSubscriber2 = _node.subscribe("Image",1, &CheckerboardDetector::image_cb2, this);
00258         _srvDetect = _node.advertiseService("Detect",&CheckerboardDetector::detect_cb,this);
00259     }
00260 
00262     // Destructor
00263     virtual ~CheckerboardDetector()
00264     {
00265     }
00266 
00268     // Camera info callback
00269     void caminfo_cb(const sensor_msgs::CameraInfoConstPtr &msg)
00270     {
00271         boost::mutex::scoped_lock lock(this->mutex);
00272 
00273         this->_camInfoMsg = *msg;
00274 
00275         // only get the camera info once <- this is dumb
00276         //this->camInfoSubscriber.shutdown();
00277         //this->camInfoSubscriber2.shutdown();
00278     }
00279     void caminfo_cb2(const sensor_msgs::CameraInfoConstPtr &msg)
00280     {
00281         ROS_WARN("The topic CameraInfo has been deprecated.  Please change your launch file to use camera_info instead.");
00282         caminfo_cb(msg);
00283     }
00284     
00285     void publishPolygonArray(const posedetection_msgs::ObjectDetection& obj)
00286     {
00287         jsk_recognition_msgs::PolygonArray polygon_array;
00288         polygon_array.header = obj.header;
00289         for (size_t i = 0; i < obj.objects.size(); i++) {
00290             geometry_msgs::Pose pose = obj.objects[i].pose;
00291             Eigen::Affine3d affine;
00292             tf::poseMsgToEigen(pose, affine);
00293             Eigen::Vector3d A_local(0, 0, 0);
00294             Eigen::Vector3d B_local((dimx - 1) * fRectSize[0], 0, 0);
00295             Eigen::Vector3d C_local((dimx - 1) * fRectSize[0], (dimy - 1) * fRectSize[1], 0);
00296             Eigen::Vector3d D_local(0, (dimy - 1) * fRectSize[1], 0);
00297             Eigen::Vector3d A_global = affine * A_local;
00298             Eigen::Vector3d B_global = affine * B_local;
00299             Eigen::Vector3d C_global = affine * C_local;
00300             Eigen::Vector3d D_global = affine * D_local;
00301             geometry_msgs::Point32 a, b, c, d;
00302             a.x = A_global[0]; a.y = A_global[1]; a.z = A_global[2];
00303             b.x = B_global[0]; b.y = B_global[1]; b.z = B_global[2];
00304             c.x = C_global[0]; c.y = C_global[1]; c.z = C_global[2];
00305             d.x = D_global[0]; d.y = D_global[1]; d.z = D_global[2];
00306             geometry_msgs::PolygonStamped polygon;
00307             polygon.header = obj.header;
00308             polygon.polygon.points.push_back(a);
00309             polygon.polygon.points.push_back(b);
00310             polygon.polygon.points.push_back(c);
00311             polygon.polygon.points.push_back(d);
00312             polygon_array.polygons.push_back(polygon);
00313         }
00314         _pubPolygonArray.publish(polygon_array);
00315     }
00316 
00317     void image_cb2(const sensor_msgs::ImageConstPtr &msg)
00318     {
00319         ROS_WARN("The topic Image has been deprecated.  Please change your launch file to use image instead.");
00320         boost::mutex::scoped_lock lock(this->mutex);
00321         ++message_throttle_counter_;
00322         if (message_throttle_counter_ % message_throttle_ == 0) {
00323             message_throttle_counter_ = 0;
00324             if( Detect(_objdetmsg,*msg,this->_camInfoMsg) ) {
00325                 if (_objdetmsg.objects.size() > 0) {
00326                     geometry_msgs::PoseStamped pose;
00327                     pose.header = _objdetmsg.header;
00328                     pose.pose = _objdetmsg.objects[0].pose;
00329                     _pubPoseStamped.publish(pose);
00330                 }
00331                 _pubDetection.publish(_objdetmsg);
00332                 publishPolygonArray(_objdetmsg);
00333             }
00334         }
00335     }
00336 
00338     // Image data callback
00339     void image_cb(const sensor_msgs::ImageConstPtr &msg)
00340     {
00341         boost::mutex::scoped_lock lock(this->mutex);
00342         ++message_throttle_counter_;
00343         if (message_throttle_counter_ % message_throttle_ == 0) {
00344             message_throttle_counter_ = 0;
00345             if( Detect(_objdetmsg,*msg,this->_camInfoMsg) ) {
00346                 if (_objdetmsg.objects.size() > 0) {
00347                     geometry_msgs::PoseStamped pose;
00348                     pose.header = _objdetmsg.header;
00349                     pose.pose = _objdetmsg.objects[0].pose;
00350                     _pubPoseStamped.publish(pose);
00351                 }
00352                 _pubDetection.publish(_objdetmsg);
00353                 publishPolygonArray(_objdetmsg);
00354             }
00355         }
00356     }
00357 
00358     bool detect_cb(posedetection_msgs::Detect::Request& req, posedetection_msgs::Detect::Response& res)
00359     {
00360         bool result = Detect(res.object_detection,req.image,req.camera_info);
00361         return result;
00362     }
00363 
00364 
00365     void subscribe( )
00366     {
00367         if ( camInfoSubscriber == NULL )
00368             camInfoSubscriber = _node.subscribe("camera_info", queue_size,
00369                                                 &CheckerboardDetector::caminfo_cb, this);
00370         if ( imageSubscriber == NULL ) {
00371             imageSubscriber = _node.subscribe("image", queue_size,
00372                                               &CheckerboardDetector::image_cb, this);
00373             if ( imageSubscriber.getTopic().find("image_rect") != std::string::npos )
00374                 ROS_WARN("topic name seems rectified, please use unrectified image"); // rectified image has 'image_rect' in topic name
00375         }
00376         if ( camInfoSubscriber2 == NULL )
00377             camInfoSubscriber2 = _node.subscribe("CameraInfo", queue_size, &CheckerboardDetector::caminfo_cb2, this);
00378         if ( imageSubscriber2 == NULL ) {
00379             imageSubscriber2 = _node.subscribe("Image", queue_size, &CheckerboardDetector::image_cb2, this);
00380             if ( imageSubscriber2.getTopic().find("image_rect") != std::string::npos )
00381                 ROS_WARN("topic name seems rectified, please use unrectified image"); // rectified image has 'image_rect' in topic name
00382         }
00383     }
00384 
00385     void unsubscribe( )
00386     {
00387         camInfoSubscriber.shutdown();
00388         camInfoSubscriber2.shutdown();
00389         imageSubscriber.shutdown();
00390         imageSubscriber2.shutdown();
00391     }
00392     
00393     void connectCb( )
00394     {
00395       boost::mutex::scoped_lock lock(this->mutex);
00396       if (_pubDetection.getNumSubscribers() == 0 && _pubCornerPoint.getNumSubscribers() == 0 &&
00397           _pubPoseStamped.getNumSubscribers() == 0 && _pubPolygonArray.getNumSubscribers() == 0)
00398         {
00399             unsubscribe();
00400         }
00401       else
00402         {
00403             subscribe();
00404         }
00405     }
00406 
00407     bool Detect(posedetection_msgs::ObjectDetection& objdetmsg,
00408                 const sensor_msgs::Image& imagemsg,
00409                 const sensor_msgs::CameraInfo& camInfoMsg)
00410     {
00411         image_geometry::PinholeCameraModel model;
00412         sensor_msgs::CameraInfo cam_info(camInfoMsg);
00413         if (cam_info.distortion_model.empty()) {
00414             cam_info.distortion_model = "plumb_bob";
00415             cam_info.D.resize(5, 0);
00416         }
00417         if (use_P) {
00418             for (size_t i = 0; i < cam_info.D.size(); i++) {
00419                 cam_info.D[i] = 0.0;
00420             }
00421         }
00422         // check all the value of R is zero or not
00423         // if zero, normalzie it
00424         if (use_P || std::equal(cam_info.R.begin() + 1, cam_info.R.end(), cam_info.R.begin())) {
00425             cam_info.R[0] = 1.0;
00426             cam_info.R[4] = 1.0;
00427             cam_info.R[8] = 1.0;
00428         }
00429         // check all the value of K is zero or not
00430         // if zero, copy all the value from P
00431         if (use_P || std::equal(cam_info.K.begin() + 1, cam_info.K.end(), cam_info.K.begin())) {
00432             cam_info.K[0] = cam_info.P[0];
00433             cam_info.K[1] = cam_info.P[1];
00434             cam_info.K[2] = cam_info.P[2];
00435             cam_info.K[3] = cam_info.P[4];
00436             cam_info.K[4] = cam_info.P[5];
00437             cam_info.K[5] = cam_info.P[6];
00438             cam_info.K[6] = cam_info.P[8];
00439             cam_info.K[7] = cam_info.P[9];
00440             cam_info.K[8] = cam_info.P[10];
00441         }
00442         model.fromCameraInfo(cam_info);
00443         cv_bridge::CvImagePtr capture_ptr;
00444         try {
00445           if (imagemsg.encoding == "32FC1") {
00446             cv_bridge::CvImagePtr float_capture
00447               = cv_bridge::toCvCopy(imagemsg,
00448                                     sensor_msgs::image_encodings::TYPE_32FC1);
00449             cv::Mat float_image = float_capture->image;
00450             cv::Mat mono_image;
00451             float_image.convertTo(mono_image, CV_8UC1);
00452             capture_ptr.reset(new cv_bridge::CvImage());
00453             capture_ptr->image = mono_image;
00454           }
00455           else {
00456             capture_ptr = cv_bridge::toCvCopy(imagemsg, sensor_msgs::image_encodings::MONO8);
00457           }
00458         } catch (cv_bridge::Exception &e) {
00459             ROS_ERROR("failed to get image %s", e.what());
00460             return false;
00461         }
00462         cv::Mat capture = capture_ptr->image;
00463         if (invert_color) {
00464             capture = cv::Mat((capture + 0.0) * 1.0 / 1.0) * 1.0;
00465             //capture = 255 - capture;
00466             cv::Mat tmp;
00467             cv::bitwise_not(capture, tmp);
00468             capture = tmp;
00469         }
00470 
00471         cv::Mat frame;
00472         
00473         if( display ) {
00474             cv::cvtColor(capture, frame, CV_GRAY2BGR);
00475         }
00476 
00477         vector<posedetection_msgs::Object6DPose> vobjects;
00478 
00479 #pragma omp parallel for schedule(dynamic,1)
00480         for(int i = 0; i < (int)vcheckers.size(); ++i) {
00481             CHECKERBOARD& cb = vcheckers[i];
00482             int ncorners, board=0;
00483             posedetection_msgs::Object6DPose objpose;
00484 
00485             // do until no more checkerboards detected
00486             while((maxboard==-1)?1:((++board)<=maxboard)) {
00487                 bool allfound = false;
00488                 if (cb.board_type == "chess") {
00489                     allfound = cv::findChessboardCorners(
00490                         capture, cb.griddims, cb.corners,
00491                         (adaptive_thresh_flag ? CV_CALIB_CB_ADAPTIVE_THRESH : 0) |
00492                         (normalize_image_flag ? CV_CALIB_CB_NORMALIZE_IMAGE : 0) |
00493                         (filter_quads_flag ? CV_CALIB_CB_FILTER_QUADS : 0) |
00494                         (fast_check_flag ? CV_CALIB_CB_FAST_CHECK : 0)
00495                         );
00496                 }
00497                 else if (cb.board_type == "circle" ||
00498                          cb.board_type == "circles") {
00499                     allfound =
00500                         cv::findCirclesGrid(capture, cb.griddims, cb.corners);
00501                 }
00502                 else if (cb.board_type == "acircle" ||
00503                          cb.board_type == "acircles") {
00504                     // sometime cv::findCirclesGrid hangs
00505                     allfound =
00506                         cv::findCirclesGrid(
00507                             capture, cb.griddims, cb.corners,
00508                             cv::CALIB_CB_ASYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING);
00509                 }
00510 
00511                 if(!allfound || cb.corners.size() != cb.grid3d.size())
00512                     break;
00513 
00514                 // remove any corners that are close to the border
00515                 const int borderthresh = 30;
00516 
00517                 for(int j = 0; j < ncorners; ++j) {
00518                     int x = cb.corners[j].x;
00519                     int y = cb.corners[j].y;
00520                     if( x < borderthresh || x > capture.cols - borderthresh ||
00521                         y < borderthresh || y > capture.rows - borderthresh )
00522                     {
00523                         allfound = false;
00524                         break;
00525                     }
00526                 }
00527 
00528                 // mark out the image
00529                 cv::Point upperleft, lowerright;
00530                 upperleft.x = lowerright.x = cb.corners[0].x;
00531                 upperleft.y = lowerright.y = cb.corners[0].y;
00532                 for(size_t j = 1; j < cb.corners.size(); ++j) {
00533                     if( upperleft.x > cb.corners[j].x ) upperleft.x = cb.corners[j].x;
00534                     if( upperleft.y > cb.corners[j].y ) upperleft.y = cb.corners[j].y;
00535                     if( lowerright.x < cb.corners[j].x ) lowerright.x = cb.corners[j].x;
00536                     if( lowerright.y < cb.corners[j].y ) lowerright.y = cb.corners[j].y;
00537                 }
00538 
00539                 float step_size =
00540                   (double)( ((upperleft.x - lowerright.x) * (upperleft.x - lowerright.x)) +
00541                             ((upperleft.y - lowerright.y) * (upperleft.y - lowerright.y)) )
00542                   /
00543                   ( ((cb.griddims.width - 1) * (cb.griddims.width - 1)) +
00544                     ((cb.griddims.height - 1) * (cb.griddims.height - 1)) );
00545 #if 0
00546                 ROS_INFO("(%d %d) - (%d %d) -> %f ",
00547                          upperleft.x, upperleft.y, lowerright.x, lowerright.y, sqrt(step_size));
00548 #endif
00549                 int size = (int)(0.5*sqrt(step_size) + 0.5);
00550 
00551                 if( allfound ) {
00552                     if (cb.board_type == "chess") { // subpixel only for chessboard
00553                         cv::cornerSubPix(capture, cb.corners,
00554                                          cv::Size(size,size), cv::Size(-1,-1),
00555                                          cv::TermCriteria(CV_TERMCRIT_ITER, 50, 1e-2));
00556                     }
00557                     objpose.pose = FindTransformation(cb.corners, cb.grid3d, cb.tlocaltrans, model, size);
00558                 }
00559 
00560 #pragma omp critical
00561                 {
00562                     if( allfound ) {
00563                         vobjects.push_back(objpose);
00564                         vobjects.back().type = vstrtypes[i];
00565                     }
00566                     cv::rectangle(capture, upperleft, lowerright,
00567                                   cv::Scalar(0,0,0), CV_FILLED);
00568                 }
00569             }
00570 
00571             //cvSaveImage("temp.jpg", pimggray);
00572         }
00573 
00574         objdetmsg.objects = vobjects;
00575         objdetmsg.header.stamp = imagemsg.header.stamp;
00576         if( frame_id.size() > 0 )
00577             objdetmsg.header.frame_id = frame_id;
00578         else
00579             objdetmsg.header.frame_id = imagemsg.header.frame_id;
00580 
00581         if( verbose > 0 )
00582             ROS_INFO("checkerboard: image: %ux%u (size=%u), num: %u, total: %.3fs",
00583                      imagemsg.width, imagemsg.height,
00584                      (unsigned int)imagemsg.data.size(), (unsigned int)objdetmsg.objects.size(),
00585                      (float)(ros::Time::now() - lasttime).toSec());
00586         lasttime = ros::Time::now();
00587 
00588         if( display ) {
00589             // draw each found checkerboard
00590             for(size_t i = 0; i < vobjects.size(); ++i) {
00591                 int itype = maptypes[vobjects[i].type];
00592                 CHECKERBOARD& cb = vcheckers[itype];
00593                 Transform tglobal;
00594                 tglobal.trans = Vector(vobjects[i].pose.position.x,vobjects[i].pose.position.y,vobjects[i].pose.position.z);
00595                 tglobal.rot = Vector(vobjects[i].pose.orientation.w,vobjects[i].pose.orientation.x,vobjects[i].pose.orientation.y, vobjects[i].pose.orientation.z);
00596                 Transform tlocal = tglobal * cb.tlocaltrans.inverse();
00597 
00598                 // draw all the points
00599                 int csize0 = std::max(circle_size_, 6);
00600                 int cwidth0 = std::max(circle_size_/3, 2);
00601                 int csize1 = std::max((2*circle_size_)/3, 4);
00602                 int csize2 = std::max(circle_size_/4, 2);
00603                 int cwidth1 = std::max(circle_size_/2, 4);
00604                 int cwidth2 = std::max(circle_size_/2, 3);
00605 
00606                 for(size_t i = 0; i < cb.grid3d.size(); ++i) {
00607                     Vector grid3d_vec(cb.grid3d[i].x, cb.grid3d[i].y, cb.grid3d[i].z);
00608                     Vector p = tlocal * grid3d_vec;
00609                     dReal fx = p.x*camInfoMsg.P[0] + p.y*camInfoMsg.P[1] + p.z*camInfoMsg.P[2] + camInfoMsg.P[3];
00610                     dReal fy = p.x*camInfoMsg.P[4] + p.y*camInfoMsg.P[5] + p.z*camInfoMsg.P[6] + camInfoMsg.P[7];
00611                     dReal fz = p.x*camInfoMsg.P[8] + p.y*camInfoMsg.P[9] + p.z*camInfoMsg.P[10] + camInfoMsg.P[11];
00612                     int x = (int)(fx/fz);
00613                     int y = (int)(fy/fz);
00614                     cv::circle(frame, cv::Point(x,y), csize0, cv::Scalar(0, 255, 0), cwidth0);
00615                     cv::circle(frame, cv::Point(x,y), csize1, cv::Scalar(64*itype,128,128), cwidth1);
00616                     cv::circle(frame, cv::Point(x,y), csize2, cv::Scalar(0,   0, 0), cwidth0);
00617                 }
00618 
00619                 cv::Point X[4];
00620 
00621                 Vector vaxes[4];
00622                 vaxes[0] = Vector(0,0,0);
00623                 vaxes[1] = Vector(axis_size_,0,0);
00624                 vaxes[2] = Vector(0,axis_size_,0);
00625                 vaxes[3] = Vector(0,0,axis_size_);
00626 
00627                 for(int i = 0; i < 4; ++i) {
00628                     Vector p = tglobal*vaxes[i];
00629                     dReal fx = p.x*camInfoMsg.P[0] + p.y*camInfoMsg.P[1] + p.z*camInfoMsg.P[2] + camInfoMsg.P[3];
00630                     dReal fy = p.x*camInfoMsg.P[4] + p.y*camInfoMsg.P[5] + p.z*camInfoMsg.P[6] + camInfoMsg.P[7];
00631                     dReal fz = p.x*camInfoMsg.P[8] + p.y*camInfoMsg.P[9] + p.z*camInfoMsg.P[10] + camInfoMsg.P[11];
00632                     X[i].x = (int)(fx/fz);
00633                     X[i].y = (int)(fy/fz);
00634                 }
00635 
00636                 cv::circle(frame, X[0], cwidth2, cv::Scalar(255,255,128), cwidth2);
00637 
00638                 // draw three lines
00639                 cv::Scalar col0(255,0,(64*itype)%256); // B
00640                 cv::Scalar col1(0,255,(64*itype)%256); // G
00641                 cv::Scalar col2((64*itype)%256,(64*itype)%256,255); // R
00642                 int axis_width_ = floor(axis_size_ / 0.03);
00643                 cv::line(frame, X[0], X[3], col0, axis_width_);
00644                 cv::line(frame, X[0], X[2], col1, axis_width_);
00645                 cv::line(frame, X[0], X[1], col2, axis_width_);
00646 
00647                 // publish X[0]
00648                 geometry_msgs::PointStamped point_msg;
00649                 point_msg.header = imagemsg.header;
00650                 point_msg.point.x = X[0].x;
00651                 point_msg.point.y = X[0].y;
00652                 point_msg.point.z = vobjects[vobjects.size() - 1].pose.position.z;
00653                 _pubCornerPoint.publish(point_msg);
00654             }
00655 
00656             cv::imshow("Checkerboard Detector",frame);
00657             cv::waitKey(1);
00658         }
00659 
00660         return true;
00661     }
00662 
00663 
00665     // FindTransformation
00666     geometry_msgs::Pose FindTransformation(
00667         const vector<cv::Point2f> &imgpts, const vector<cv::Point3f> &objpts,
00668         const Transform& tlocal,
00669         const image_geometry::PinholeCameraModel& model,
00670         double cell_size = 1.0)
00671     {
00672         geometry_msgs::Pose pose;
00673         Transform tchecker;
00674         cv::Mat R3_mat, T3_mat;
00675         cv::solvePnP(objpts, imgpts,
00676                      model.intrinsicMatrix(),
00677                      model.distortionCoeffs(),
00678                      R3_mat, T3_mat, false);
00679         double fR3[3];
00680         for (size_t i = 0; i < 3; i++) {
00681           fR3[i] = R3_mat.at<double>(i);
00682         }
00683         tchecker.trans.x = T3_mat.at<double>(0);
00684         tchecker.trans.y = T3_mat.at<double>(1);
00685         tchecker.trans.z = T3_mat.at<double>(2);
00686         double fang = sqrt(fR3[0]*fR3[0] + fR3[1]*fR3[1] + fR3[2]*fR3[2]);
00687         if( fang >= 1e-6 ) {
00688             double fmult = sin(fang/2)/fang;
00689             tchecker.rot = Vector(cos(fang/2),fR3[0]*fmult, fR3[1]*fmult, fR3[2]*fmult);
00690         }
00691 
00692         // check if points are in truth position
00693         cv::Mat projpoints(objpts.size(), 2, CV_32FC1);
00694         projectPoints(objpts, R3_mat, T3_mat, model.intrinsicMatrix(), model.distortionCoeffs(), projpoints);
00695         const cv::Point2f* projpoints_ptr = projpoints.ptr<cv::Point2f>();
00696         float max_diff_norm = -10;
00697         for ( size_t i = 0; i < objpts.size(); ++i){
00698             float diff_norm = (float)norm(imgpts[i] - projpoints_ptr[i]);
00699             if (diff_norm > max_diff_norm) {
00700                 max_diff_norm = diff_norm;
00701             }
00702         }
00703         ROS_DEBUG("max error of cells: %d", max_diff_norm/cell_size);
00704         if (max_diff_norm/cell_size > 0.02) { // 2%
00705             ROS_WARN("Large error %f detected, please check (1): checker board is on plane, (2): you uses rectified image", max_diff_norm/cell_size);
00706         }
00707 
00708         Transform tglobal = tchecker*tlocal;
00709         pose.position.x = tglobal.trans.x;
00710         pose.position.y = tglobal.trans.y;
00711         pose.position.z = tglobal.trans.z;
00712         pose.orientation.x = tglobal.rot.y;
00713         pose.orientation.y = tglobal.rot.z;
00714         pose.orientation.z = tglobal.rot.w;
00715         pose.orientation.w = tglobal.rot.x;
00716         return pose;
00717     }
00718 
00720     // Dynamic Reconfigure
00721     void configCallback(Config &config, uint32_t level)
00722     {
00723         boost::mutex::scoped_lock lock(this->mutex);
00724         adaptive_thresh_flag = config.adaptive_thresh;
00725         normalize_image_flag = config.normalize_image;
00726         filter_quads_flag = config.filter_quads;
00727         fast_check_flag = config.fast_check;
00728     }
00729 };
00730 
00732 // MAIN
00733 int main(int argc, char **argv)
00734 {
00735     ros::init(argc,argv,"checkerboard_detector");
00736     if( !ros::master::check() )
00737         return 1;
00738   
00739     CheckerboardDetector cd;
00740     ros::spin();
00741 
00742     return 0;
00743 }


checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Sun Oct 8 2017 02:42:45