checkerboard_calibration.cpp
Go to the documentation of this file.
00001 // Software License Agreement (BSD License)
00002 // Copyright (c) 2008, Rosen Diankov
00003 // Redistribution and use in source and binary forms, with or without
00004 // modification, are permitted provided that the following conditions are met:
00005 //   * Redistributions of source code must retain the above copyright notice,
00006 //     this list of conditions and the following disclaimer.
00007 //   * Redistributions in binary form must reproduce the above copyright
00008 //     notice, this list of conditions and the following disclaimer in the
00009 //     documentation and/or other materials provided with the distribution.
00010 //   * The name of the author may not be used to endorse or promote products
00011 //     derived from this software without specific prior written permission.
00012 //
00013 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00014 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00015 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00016 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00017 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00018 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00019 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00020 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00021 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00022 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00023 // POSSIBILITY OF SUCH DAMAGE.
00024 //
00025 // author: Rosen Diankov
00026 #include <cstdio>
00027 #include <vector>
00028 #include <ros/ros.h>
00029 
00030 #include <boost/thread/mutex.hpp>
00031 
00032 #include "opencv/cv.h"
00033 #include "opencv/highgui.h"
00034 #include "cv_bridge/CvBridge.h"
00035 #include "sensor_msgs/Image.h"
00036 #include "math.h"
00037 
00038 #include <sys/timeb.h>    // ftime(), struct timeb
00039 #include <sys/time.h>
00040 
00041 using namespace std;
00042 using namespace ros;
00043 
00044 template <typename T>
00045 T SQR(T t) { return t*t; }
00046 
00047 
00048 class CheckerboardCalibration
00049 {
00050 public:
00051     ros::NodeHandle _nh;
00052     ros::Subscriber _sub, _sub2;
00053     struct CHECKERBOARD
00054     {
00055         CvSize griddims; 
00056         vector<RaveVector<float> > grid3d;
00057         vector<CvPoint2D32f> corners;
00058     };
00059 
00060     sensor_msgs::CvBridge _cvbridge;
00061     
00062     int display;
00063     CHECKERBOARD _checkerboard; // grid points for every checkerboard
00064     IplImage* frame;
00065     vector<CvPoint2D32f> _vAllPoints; 
00066     vector<int> _vNumPointsPerImage;
00067 
00068     CvMat *_intrinsic_matrix, *_distortion_coeffs; // intrinsic matrices
00069     IplImage* _pUndistortionMapX, *_pUndistortionMapY;
00070     TransformMatrix _tProjection; // project matrix
00071     bool _bHasCalibration, _bClearData, _bTakeObservation;
00072 
00073   CheckerboardCalibration() : frame(NULL), _intrinsic_matrix(NULL), _distortion_coeffs(NULL), _pUndistortionMapX(NULL), _pUndistortionMapY(NULL), _bHasCalibration(false), _bClearData(false), _bTakeObservation(false)
00074     {
00075         _nh.param("display",display,1);
00076 
00077         int dimx, dimy;
00078         double fRectSize[2];
00079         string type;
00080         
00081         if( !_nh.getParam("grid_size_x",dimx) ) {
00082             ROS_ERROR("grid_size_x not defined");
00083             throw;
00084         }
00085         if( !_nh.getParam("grid_size_y",dimy) ) {
00086             ROS_ERROR("grid_size_y not defined");
00087             throw;
00088         }
00089         if( !_nh.getParam("rect_size_x",fRectSize[0]) ) {
00090             ROS_ERROR("rect_size_x not defined");
00091             throw;
00092         }
00093         if( !_nh.getParam("rect_size_y",fRectSize[1]) ) {
00094             ROS_ERROR("rect_size_y not defined");
00095             throw;
00096         }
00097 
00098         _checkerboard.griddims = cvSize(dimx,dimy);
00099         _checkerboard.grid3d.resize(dimx*dimy);
00100         int j=0;
00101         for(int y=0; y<dimy; ++y)
00102             for(int x=0; x<dimx; ++x)
00103                 _checkerboard.grid3d[j++] = Vector(x*fRectSize[0], y*fRectSize[1], 0);
00104 
00105         if( _intrinsic_matrix == NULL )
00106             _intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);
00107         if( _distortion_coeffs == NULL )
00108             _distortion_coeffs = cvCreateMat(4,1,CV_32FC1);
00109 
00110         if( display ) {
00111             cvNamedWindow("Checkerboard Detector", CV_WINDOW_AUTOSIZE);
00112             cvSetMouseCallback("Checkerboard Detector", MouseCallback, this);
00113             cvNamedWindow("Calibration Result", CV_WINDOW_AUTOSIZE);
00114             cvSetMouseCallback("Calibration Result", MouseCallback, this);
00115             cvStartWindowThread();
00116         }
00117 
00118         _sub = _nh.subscribe("image", 1, &CheckerboardCalibration::image_cb,this);
00119         _sub2 = _nh.subscribe("Image", 1, &CheckerboardCalibration::image_cb2,this);
00120     }
00121     ~CheckerboardCalibration()
00122     {
00123         if( frame )
00124             cvReleaseImage(&frame);
00125         if( _intrinsic_matrix )
00126             cvReleaseMat(&_intrinsic_matrix);
00127         if( _distortion_coeffs )
00128             cvReleaseMat(&_distortion_coeffs);
00129         if( _pUndistortionMapX )
00130             cvReleaseImage(&_pUndistortionMapX);
00131         if( _pUndistortionMapY )
00132             cvReleaseImage(&_pUndistortionMapY);
00133     }
00134 
00135 private:
00136     void image_cb2(const sensor_msgs::ImageConstPtr& image_msg)
00137     {
00138         image_cb(image_msg);
00139     }
00140     void image_cb(const sensor_msgs::ImageConstPtr& image_msg)
00141     {
00142         if( !_cvbridge.fromImage(*image_msg, "mono8") ) {
00143             ROS_ERROR("failed to get image");
00144             return;
00145         }
00146 
00147         IplImage *pimggray = _cvbridge.toIpl();
00148         if( display ) {
00149             // copy the raw image
00150             if( frame != NULL && (frame->width != (int)pimggray->width || frame->height != (int)pimggray->height) ) {
00151                 cvReleaseImage(&frame);
00152                 frame = NULL;
00153             }
00154             if( frame == NULL ) 
00155                 frame = cvCreateImage(cvSize(pimggray->width,pimggray->height),IPL_DEPTH_8U, 3);
00156 
00157             cvCvtColor(pimggray,frame,CV_GRAY2RGB);
00158         }
00159 
00160         int ncorners;
00161         _checkerboard.corners.resize(_checkerboard.grid3d.size()+64);
00162         int allfound = cvFindChessboardCorners( pimggray, _checkerboard.griddims, &_checkerboard.corners[0],
00163                                                 &ncorners, CV_CALIB_CB_ADAPTIVE_THRESH );
00164         _checkerboard.corners.resize(ncorners);        
00165         
00166         if(allfound && ncorners == (int)_checkerboard.grid3d.size()) {
00167         
00168             cvFindCornerSubPix(pimggray, &_checkerboard.corners[0], _checkerboard.corners.size(), cvSize(5,5),cvSize(-1,-1),
00169                                cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
00170         }
00171         else
00172             _checkerboard.corners.resize(0);
00173 
00174         if( _bClearData ) {
00175             ROS_INFO("clearing current calibration data");
00176             _vAllPoints.resize(0);
00177             _vNumPointsPerImage.resize(0);
00178             _bHasCalibration = false;
00179             _bClearData = false;
00180         }
00181 
00182         if( _bTakeObservation ) {
00183             _bTakeObservation = false;
00184 
00185             if(allfound && ncorners == (int)_checkerboard.grid3d.size()) {
00186                 
00187                 for(vector<CvPoint2D32f>::iterator it = _checkerboard.corners.begin(); it != _checkerboard.corners.end(); ++it)
00188                     _vAllPoints.push_back(*it);
00189                 _vNumPointsPerImage.push_back(_checkerboard.corners.size());
00190                 
00191                 ROS_INFO("calibrating");
00192             
00193                 CvMat object_points, image_points, point_counts;
00194             
00195                 vector<float> vobject_points;
00196                 vobject_points.reserve(_vNumPointsPerImage.size()*3*_checkerboard.grid3d.size());
00197                 for(size_t j = 0; j < _vNumPointsPerImage.size(); ++j) {
00198                     for(size_t i = 0; i < _checkerboard.grid3d.size(); ++i) {
00199                         vobject_points.push_back(_checkerboard.grid3d[i].x);
00200                         vobject_points.push_back(_checkerboard.grid3d[i].y);
00201                         vobject_points.push_back(_checkerboard.grid3d[i].z);
00202                     }
00203                 }
00204 
00205                 cvInitMatHeader(&object_points, vobject_points.size()/3,3,CV_32FC1, &vobject_points[0]);
00206                 cvInitMatHeader(&image_points, _vAllPoints.size(), 2, CV_32FC1, &_vAllPoints[0]);
00207                 cvInitMatHeader(&point_counts, _vNumPointsPerImage.size(), 1, CV_32SC1, &_vNumPointsPerImage[0]);
00208                 
00209                 CvMat* rotation_vectors = cvCreateMat(_vNumPointsPerImage.size(),3,CV_32FC1);
00210                 CvMat* translation_vectors = cvCreateMat(_vNumPointsPerImage.size(),3,CV_32FC1);
00211                 
00212                 cvCalibrateCamera2(&object_points, &image_points,
00213                                    &point_counts, cvSize(frame->width,frame->height),
00214                                    _intrinsic_matrix, _distortion_coeffs,
00215                                    rotation_vectors, translation_vectors, 0);
00216 
00217                 double err = 0;
00218                 int off = 0;
00219                 for(size_t i = 0; i < _vNumPointsPerImage.size(); ++i) {
00220                     CvMat cur_object_points, rotation_vector, translation_vector;
00221 
00222                     cvInitMatHeader(&cur_object_points, _vNumPointsPerImage[i],3,CV_32FC1, &vobject_points[0]);
00223                     cvInitMatHeader(&rotation_vector, 3, 1, CV_32FC1, &rotation_vectors->data.fl[3*i]);
00224                     cvInitMatHeader(&translation_vector, 3, 1, CV_32FC1, &translation_vectors->data.fl[3*i]);
00225                     CvMat* new_image_points = cvCreateMat(_vNumPointsPerImage[i], 2, CV_32FC1);
00226                     cvProjectPoints2(&cur_object_points, &rotation_vector, &translation_vector, _intrinsic_matrix,
00227                                      _distortion_coeffs, new_image_points);
00228 
00229                     for(int j = 0; j < _vNumPointsPerImage[i]; ++j)
00230                         err += SQR(new_image_points->data.fl[2*j]-_vAllPoints[off+j].x) + SQR(new_image_points->data.fl[2*j+1]-_vAllPoints[off+j].y);
00231 
00232                     cvReleaseMat(&new_image_points);
00233                     off += _vNumPointsPerImage[i];
00234                 }
00235                 err = sqrt(err/(double)_vAllPoints.size());
00236 
00237                 ROS_INFO("calibration done, reprojection error = %f", (float)err);
00238                 ROS_INFO("Intrinsic Matrix:");
00239                 for(int i = 0; i < 3; ++i) {
00240                     ROS_INFO("%15f %15f %15f", _intrinsic_matrix->data.fl[3*i], _intrinsic_matrix->data.fl[3*i+1], _intrinsic_matrix->data.fl[3*i+2]);
00241                     _tProjection.m[4*i+0] = _intrinsic_matrix->data.fl[3*i+0];
00242                     _tProjection.m[4*i+1] = _intrinsic_matrix->data.fl[3*i+1];
00243                     _tProjection.m[4*i+2] = _intrinsic_matrix->data.fl[3*i+2];
00244                 }
00245                 ROS_INFO("distortion: %f %f %f %f", _distortion_coeffs->data.fl[0], _distortion_coeffs->data.fl[1], _distortion_coeffs->data.fl[2], _distortion_coeffs->data.fl[3]);
00246 
00247                 cvReleaseMat(&rotation_vectors);
00248                 cvReleaseMat(&translation_vectors);
00249 
00250                 // create undistortion maps
00251                 if( _pUndistortionMapX == NULL )
00252                     _pUndistortionMapX = cvCreateImage( cvSize(frame->width, frame->height), IPL_DEPTH_32F, 1);
00253                 if( _pUndistortionMapY == NULL )
00254                     _pUndistortionMapY = cvCreateImage( cvSize(frame->width, frame->height), IPL_DEPTH_32F, 1);
00255 
00256 #ifdef HAVE_CV_UNDISTORT_RECTIFY_MAP // for newer opencv versions, *have* to use cvInitUndistortRectifyMap
00257                 float feye[9] = {1,0,0,0,1,0,0,0,1};
00258                 CvMat eye = cvMat(3,3,CV_32FC1, feye);
00259                 cvInitUndistortRectifyMap(_intrinsic_matrix,_distortion_coeffs,&eye,_intrinsic_matrix,_pUndistortionMapX, _pUndistortionMapY);
00260 #else
00261                 cvInitUndistortMap( _intrinsic_matrix, _distortion_coeffs, _pUndistortionMapX, _pUndistortionMapY );
00262 #endif
00263                 _bHasCalibration = true;
00264             }
00265             else
00266                 ROS_WARN("no checkerboard found");
00267         }
00268 
00269         if( display ) {            
00270             if( _bHasCalibration ) {
00271                 IplImage* frame_undist = cvCloneImage(frame);
00272                 IplImage* pimggray_undist = cvCloneImage(pimggray);
00273 
00274                 cvRemap( pimggray, pimggray_undist, _pUndistortionMapX, _pUndistortionMapY, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS);
00275                 cvRemap( frame, frame_undist, _pUndistortionMapX, _pUndistortionMapY, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS);
00276 
00277                 int ncorners;
00278                 vector<CvPoint2D32f> corners(_checkerboard.grid3d.size()+64);
00279                 int allfound = cvFindChessboardCorners( pimggray_undist, _checkerboard.griddims, &corners[0],
00280                                                         &ncorners, CV_CALIB_CB_ADAPTIVE_THRESH );
00281                 corners.resize(ncorners);
00282         
00283                 if(allfound && ncorners == (int)_checkerboard.grid3d.size()) {
00284                     //cvCvtColor(pimggray,frame,CV_GRAY2RGB);
00285                     cvFindCornerSubPix(pimggray_undist, &corners[0], corners.size(), cvSize(5,5),cvSize(-1,-1),
00286                                        cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
00287 
00288 
00289                     RaveTransform<float> tlocal = FindTransformation(corners, _checkerboard.grid3d);
00290 
00291                     CvSize& s = _checkerboard.griddims;
00292                     CvPoint X[4];
00293                     int inds[4] = {0, s.width-1, s.width*(s.height-1), s.width*s.height-1 };
00294             
00295                     for(int i = 0; i < 4; ++i) {
00296                         Vector p = _tProjection * tlocal * _checkerboard.grid3d[inds[i]];
00297                         X[i].x = (int)(p.x/p.z);
00298                         X[i].y = (int)(p.y/p.z);
00299                     }
00300                 
00301                     // draw two lines
00302                     CvScalar col0 = CV_RGB(255,0,0);
00303                     CvScalar col1 = CV_RGB(0,255,0);
00304                     cvLine(frame_undist, X[0], X[1], col0, 1);
00305                     cvLine(frame_undist, X[0], X[2], col1, 1);
00306                 
00307                     // draw all the points
00308                     for(size_t i = 0; i < _checkerboard.grid3d.size(); ++i) {
00309                         Vector p = _tProjection * tlocal * _checkerboard.grid3d[i];
00310                         int x = (int)(p.x/p.z);
00311                         int y = (int)(p.y/p.z);
00312                         cvCircle(frame_undist, cvPoint(x,y), 6, CV_RGB(0,0,0), 2);
00313                         cvCircle(frame_undist, cvPoint(x,y), 2, CV_RGB(0,0,0), 2);
00314                         cvCircle(frame_undist, cvPoint(x,y), 4, CV_RGB(128,128,0), 3);
00315                     }
00316                 
00317                     cvCircle(frame_undist, X[0], 3, CV_RGB(255,255,128), 3);
00318                 }
00319 
00320                 cvShowImage("Calibration Result",frame_undist);
00321                 cvReleaseImage(&frame_undist);
00322             }
00323 
00324             if(allfound && ncorners == (int)_checkerboard.grid3d.size())
00325                 cvDrawChessboardCorners(frame, _checkerboard.griddims, &_checkerboard.corners[0], _checkerboard.corners.size(), 1);
00326             
00327             cvShowImage("Checkerboard Detector",frame);
00328         }
00329     }
00330 
00331     Transform FindTransformation(const vector<CvPoint2D32f> &imgpts, const vector<Vector> &objpts)
00332     {
00333         CvMat *objpoints = cvCreateMat(3,objpts.size(),CV_32FC1);
00334         for(size_t i=0; i<objpts.size(); ++i) {
00335             cvSetReal2D(objpoints, 0,i, objpts[i].x);
00336             cvSetReal2D(objpoints, 1,i, objpts[i].y);
00337             cvSetReal2D(objpoints, 2,i, objpts[i].z);
00338         }
00339         
00340         RaveTransform<float> pose;
00341         float fR3[3];
00342         CvMat R3, T3;
00343         cvInitMatHeader(&R3, 3, 1, CV_32FC1, fR3);
00344         cvInitMatHeader(&T3, 3, 1, CV_32FC1, &pose.trans.x);
00345 
00346         float kc[4] = {0};
00347         CvMat kcmat;
00348         cvInitMatHeader(&kcmat,1,4,CV_32FC1,kc);
00349 
00350         CvMat img_points;
00351         cvInitMatHeader(&img_points, 1,imgpts.size(), CV_32FC2, const_cast<CvPoint2D32f*>(&imgpts[0]));
00352     
00353         cvFindExtrinsicCameraParams2(objpoints, &img_points, _intrinsic_matrix, &kcmat, &R3, &T3);
00354         cvReleaseMat(&objpoints);
00355         
00356         double fang = sqrt(fR3[0]*fR3[0] + fR3[1]*fR3[1] + fR3[2]*fR3[2]);
00357         if( fang < 1e-6 ) {
00358             pose.rot.x = 1;
00359             pose.rot.y = 0;
00360             pose.rot.z = 0;
00361             pose.rot.w = 0;
00362         }
00363         else {
00364             double fmult = sin(fang/2)/fang;
00365             pose.rot.x = cos(fang/2);
00366             pose.rot.y = fR3[0]*fmult;
00367             pose.rot.z = fR3[1]*fmult;
00368             pose.rot.w = fR3[2]*fmult;
00369         }
00370 
00371         return pose;
00372     }
00373 
00374     static void MouseCallback(int event, int x, int y, int flags, void* param)
00375     {
00376         ((CheckerboardCalibration*)param)->_MouseCallback(event, x, y, flags);
00377     }
00378 
00379     void _MouseCallback(int event, int x, int y, int flags)
00380     {
00381         switch(event) {
00382         case CV_EVENT_RBUTTONDOWN:
00383             _bClearData = true;
00384             break;
00385         case CV_EVENT_MBUTTONDOWN:
00386             _bTakeObservation = true;
00387             break;
00388         }
00389     }
00390 };
00391 
00392 int main(int argc, char **argv)
00393 {
00394     ros::init(argc,argv,"checkerboard_calibration");
00395     boost::shared_ptr<CheckerboardCalibration> checker(new CheckerboardCalibration());
00396     
00397     ros::spin();
00398     checker.reset();
00399     return 0;
00400 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


checkerboard_detector
Author(s): Rosen Diankov (rdiankov@cs.cmu.edu)
autogenerated on Sat Mar 23 2013 18:37:21