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.hpp"
00033 #include "opencv/highgui.h"
00034 #include "cv_bridge/cv_bridge.h"
00035 #include "sensor_msgs/image_encodings.h"
00036 #include "sensor_msgs/Image.h"
00037 #include "math.h"
00038 
00039 #include <sys/timeb.h>    // ftime(), struct timeb
00040 #include <sys/time.h>
00041 
00042 using namespace std;
00043 using namespace ros;
00044 
00045 template <typename T>
00046 T SQR(T t) { return t*t; }
00047 
00048 
00049 class CheckerboardCalibration
00050 {
00051 public:
00052     ros::NodeHandle _nh;
00053     ros::Subscriber _sub, _sub2;
00054     struct CHECKERBOARD
00055     {
00056         CvSize griddims; 
00057         vector<RaveVector<float> > grid3d;
00058         vector<CvPoint2D32f> corners;
00059     };
00060 
00061     cv_bridge::CvImagePtr _cvbridge;
00062     
00063     int display;
00064     CHECKERBOARD _checkerboard; // grid points for every checkerboard
00065     IplImage* frame;
00066     vector<CvPoint2D32f> _vAllPoints; 
00067     vector<int> _vNumPointsPerImage;
00068 
00069     CvMat *_intrinsic_matrix, *_distortion_coeffs; // intrinsic matrices
00070     IplImage* _pUndistortionMapX, *_pUndistortionMapY;
00071     TransformMatrix _tProjection; // project matrix
00072     bool _bHasCalibration, _bClearData, _bTakeObservation;
00073 
00074   CheckerboardCalibration() : frame(NULL), _intrinsic_matrix(NULL), _distortion_coeffs(NULL), _pUndistortionMapX(NULL), _pUndistortionMapY(NULL), _bHasCalibration(false), _bClearData(false), _bTakeObservation(false)
00075     {
00076         _nh.param("display",display,1);
00077 
00078         int dimx, dimy;
00079         double fRectSize[2];
00080         string type;
00081         
00082         if( !_nh.getParam("grid_size_x",dimx) ) {
00083             ROS_ERROR("grid_size_x not defined");
00084             throw;
00085         }
00086         if( !_nh.getParam("grid_size_y",dimy) ) {
00087             ROS_ERROR("grid_size_y not defined");
00088             throw;
00089         }
00090         if( !_nh.getParam("rect_size_x",fRectSize[0]) ) {
00091             ROS_ERROR("rect_size_x not defined");
00092             throw;
00093         }
00094         if( !_nh.getParam("rect_size_y",fRectSize[1]) ) {
00095             ROS_ERROR("rect_size_y not defined");
00096             throw;
00097         }
00098 
00099         _checkerboard.griddims = cvSize(dimx,dimy);
00100         _checkerboard.grid3d.resize(dimx*dimy);
00101         int j=0;
00102         for(int y=0; y<dimy; ++y)
00103             for(int x=0; x<dimx; ++x)
00104                 _checkerboard.grid3d[j++] = Vector(x*fRectSize[0], y*fRectSize[1], 0);
00105 
00106         if( _intrinsic_matrix == NULL )
00107             _intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);
00108         if( _distortion_coeffs == NULL )
00109             _distortion_coeffs = cvCreateMat(4,1,CV_32FC1);
00110 
00111         if( display ) {
00112             cvNamedWindow("Checkerboard Detector", CV_WINDOW_AUTOSIZE);
00113             cvSetMouseCallback("Checkerboard Detector", MouseCallback, this);
00114             cvNamedWindow("Calibration Result", CV_WINDOW_AUTOSIZE);
00115             cvSetMouseCallback("Calibration Result", MouseCallback, this);
00116             cvStartWindowThread();
00117         }
00118 
00119         _sub = _nh.subscribe("image", 1, &CheckerboardCalibration::image_cb,this);
00120         _sub2 = _nh.subscribe("Image", 1, &CheckerboardCalibration::image_cb2,this);
00121     }
00122     ~CheckerboardCalibration()
00123     {
00124         if( frame )
00125             cvReleaseImage(&frame);
00126         if( _intrinsic_matrix )
00127             cvReleaseMat(&_intrinsic_matrix);
00128         if( _distortion_coeffs )
00129             cvReleaseMat(&_distortion_coeffs);
00130         if( _pUndistortionMapX )
00131             cvReleaseImage(&_pUndistortionMapX);
00132         if( _pUndistortionMapY )
00133             cvReleaseImage(&_pUndistortionMapY);
00134     }
00135 
00136 private:
00137     void image_cb2(const sensor_msgs::ImageConstPtr& image_msg)
00138     {
00139         image_cb(image_msg);
00140     }
00141     void image_cb(const sensor_msgs::ImageConstPtr& image_msg)
00142     {
00143         try {
00144             _cvbridge = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::MONO8);
00145         } catch (cv_bridge::Exception &e) {
00146             ROS_ERROR("failed to get image");
00147             return;
00148         }
00149 
00150         IplImage imggray = _cvbridge->image;
00151         IplImage *pimggray = &imggray;
00152         if( display ) {
00153             // copy the raw image
00154             if( frame != NULL && (frame->width != (int)pimggray->width || frame->height != (int)pimggray->height) ) {
00155                 cvReleaseImage(&frame);
00156                 frame = NULL;
00157             }
00158             if( frame == NULL ) 
00159                 frame = cvCreateImage(cvSize(pimggray->width,pimggray->height),IPL_DEPTH_8U, 3);
00160 
00161             cvCvtColor(pimggray,frame,CV_GRAY2RGB);
00162         }
00163 
00164         int ncorners;
00165         _checkerboard.corners.resize(_checkerboard.grid3d.size()+64);
00166         int allfound = cvFindChessboardCorners( pimggray, _checkerboard.griddims, &_checkerboard.corners[0],
00167                                                 &ncorners, CV_CALIB_CB_ADAPTIVE_THRESH );
00168         _checkerboard.corners.resize(ncorners);        
00169         
00170         if(allfound && ncorners == (int)_checkerboard.grid3d.size()) {
00171         
00172             cvFindCornerSubPix(pimggray, &_checkerboard.corners[0], _checkerboard.corners.size(), cvSize(5,5),cvSize(-1,-1),
00173                                cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
00174         }
00175         else
00176             _checkerboard.corners.resize(0);
00177 
00178         if( _bClearData ) {
00179             ROS_INFO("clearing current calibration data");
00180             _vAllPoints.resize(0);
00181             _vNumPointsPerImage.resize(0);
00182             _bHasCalibration = false;
00183             _bClearData = false;
00184         }
00185 
00186         if( _bTakeObservation ) {
00187             _bTakeObservation = false;
00188 
00189             if(allfound && ncorners == (int)_checkerboard.grid3d.size()) {
00190                 
00191                 for(vector<CvPoint2D32f>::iterator it = _checkerboard.corners.begin(); it != _checkerboard.corners.end(); ++it)
00192                     _vAllPoints.push_back(*it);
00193                 _vNumPointsPerImage.push_back(_checkerboard.corners.size());
00194                 
00195                 ROS_INFO("calibrating");
00196             
00197                 CvMat object_points, image_points, point_counts;
00198             
00199                 vector<float> vobject_points;
00200                 vobject_points.reserve(_vNumPointsPerImage.size()*3*_checkerboard.grid3d.size());
00201                 for(size_t j = 0; j < _vNumPointsPerImage.size(); ++j) {
00202                     for(size_t i = 0; i < _checkerboard.grid3d.size(); ++i) {
00203                         vobject_points.push_back(_checkerboard.grid3d[i].x);
00204                         vobject_points.push_back(_checkerboard.grid3d[i].y);
00205                         vobject_points.push_back(_checkerboard.grid3d[i].z);
00206                     }
00207                 }
00208 
00209                 cvInitMatHeader(&object_points, vobject_points.size()/3,3,CV_32FC1, &vobject_points[0]);
00210                 cvInitMatHeader(&image_points, _vAllPoints.size(), 2, CV_32FC1, &_vAllPoints[0]);
00211                 cvInitMatHeader(&point_counts, _vNumPointsPerImage.size(), 1, CV_32SC1, &_vNumPointsPerImage[0]);
00212                 
00213                 CvMat* rotation_vectors = cvCreateMat(_vNumPointsPerImage.size(),3,CV_32FC1);
00214                 CvMat* translation_vectors = cvCreateMat(_vNumPointsPerImage.size(),3,CV_32FC1);
00215                 
00216                 cvCalibrateCamera2(&object_points, &image_points,
00217                                    &point_counts, cvSize(frame->width,frame->height),
00218                                    _intrinsic_matrix, _distortion_coeffs,
00219                                    rotation_vectors, translation_vectors, 0);
00220 
00221                 double err = 0;
00222                 int off = 0;
00223                 for(size_t i = 0; i < _vNumPointsPerImage.size(); ++i) {
00224                     CvMat cur_object_points, rotation_vector, translation_vector;
00225 
00226                     cvInitMatHeader(&cur_object_points, _vNumPointsPerImage[i],3,CV_32FC1, &vobject_points[0]);
00227                     cvInitMatHeader(&rotation_vector, 3, 1, CV_32FC1, &rotation_vectors->data.fl[3*i]);
00228                     cvInitMatHeader(&translation_vector, 3, 1, CV_32FC1, &translation_vectors->data.fl[3*i]);
00229                     CvMat* new_image_points = cvCreateMat(_vNumPointsPerImage[i], 2, CV_32FC1);
00230                     cvProjectPoints2(&cur_object_points, &rotation_vector, &translation_vector, _intrinsic_matrix,
00231                                      _distortion_coeffs, new_image_points);
00232 
00233                     for(int j = 0; j < _vNumPointsPerImage[i]; ++j)
00234                         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);
00235 
00236                     cvReleaseMat(&new_image_points);
00237                     off += _vNumPointsPerImage[i];
00238                 }
00239                 err = sqrt(err/(double)_vAllPoints.size());
00240 
00241                 ROS_INFO("calibration done, reprojection error = %f", (float)err);
00242                 ROS_INFO("Intrinsic Matrix:");
00243                 for(int i = 0; i < 3; ++i) {
00244                     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]);
00245                     _tProjection.m[4*i+0] = _intrinsic_matrix->data.fl[3*i+0];
00246                     _tProjection.m[4*i+1] = _intrinsic_matrix->data.fl[3*i+1];
00247                     _tProjection.m[4*i+2] = _intrinsic_matrix->data.fl[3*i+2];
00248                 }
00249                 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]);
00250 
00251                 cvReleaseMat(&rotation_vectors);
00252                 cvReleaseMat(&translation_vectors);
00253 
00254                 // create undistortion maps
00255                 if( _pUndistortionMapX == NULL )
00256                     _pUndistortionMapX = cvCreateImage( cvSize(frame->width, frame->height), IPL_DEPTH_32F, 1);
00257                 if( _pUndistortionMapY == NULL )
00258                     _pUndistortionMapY = cvCreateImage( cvSize(frame->width, frame->height), IPL_DEPTH_32F, 1);
00259 
00260 #ifdef HAVE_CV_UNDISTORT_RECTIFY_MAP // for newer opencv versions, *have* to use cvInitUndistortRectifyMap
00261                 float feye[9] = {1,0,0,0,1,0,0,0,1};
00262                 CvMat eye = cvMat(3,3,CV_32FC1, feye);
00263                 cvInitUndistortRectifyMap(_intrinsic_matrix,_distortion_coeffs,&eye,_intrinsic_matrix,_pUndistortionMapX, _pUndistortionMapY);
00264 #else
00265                 cvInitUndistortMap( _intrinsic_matrix, _distortion_coeffs, _pUndistortionMapX, _pUndistortionMapY );
00266 #endif
00267                 _bHasCalibration = true;
00268             }
00269             else
00270                 ROS_WARN("no checkerboard found");
00271         }
00272 
00273         if( display ) {            
00274             if( _bHasCalibration ) {
00275                 IplImage* frame_undist = cvCloneImage(frame);
00276                 IplImage* pimggray_undist = cvCloneImage(pimggray);
00277 
00278                 cvRemap( pimggray, pimggray_undist, _pUndistortionMapX, _pUndistortionMapY, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS);
00279                 cvRemap( frame, frame_undist, _pUndistortionMapX, _pUndistortionMapY, CV_INTER_LINEAR + CV_WARP_FILL_OUTLIERS);
00280 
00281                 int ncorners;
00282                 vector<CvPoint2D32f> corners(_checkerboard.grid3d.size()+64);
00283                 int allfound = cvFindChessboardCorners( pimggray_undist, _checkerboard.griddims, &corners[0],
00284                                                         &ncorners, CV_CALIB_CB_ADAPTIVE_THRESH );
00285                 corners.resize(ncorners);
00286         
00287                 if(allfound && ncorners == (int)_checkerboard.grid3d.size()) {
00288                     //cvCvtColor(pimggray,frame,CV_GRAY2RGB);
00289                     cvFindCornerSubPix(pimggray_undist, &corners[0], corners.size(), cvSize(5,5),cvSize(-1,-1),
00290                                        cvTermCriteria(CV_TERMCRIT_ITER,20,1e-2));
00291 
00292 
00293                     RaveTransform<float> tlocal = FindTransformation(corners, _checkerboard.grid3d);
00294 
00295                     CvSize& s = _checkerboard.griddims;
00296                     CvPoint X[4];
00297                     int inds[4] = {0, s.width-1, s.width*(s.height-1), s.width*s.height-1 };
00298             
00299                     for(int i = 0; i < 4; ++i) {
00300                         Vector p = _tProjection * tlocal * _checkerboard.grid3d[inds[i]];
00301                         X[i].x = (int)(p.x/p.z);
00302                         X[i].y = (int)(p.y/p.z);
00303                     }
00304                 
00305                     // draw two lines
00306                     CvScalar col0 = CV_RGB(255,0,0);
00307                     CvScalar col1 = CV_RGB(0,255,0);
00308                     cvLine(frame_undist, X[0], X[1], col0, 1);
00309                     cvLine(frame_undist, X[0], X[2], col1, 1);
00310                 
00311                     // draw all the points
00312                     for(size_t i = 0; i < _checkerboard.grid3d.size(); ++i) {
00313                         Vector p = _tProjection * tlocal * _checkerboard.grid3d[i];
00314                         int x = (int)(p.x/p.z);
00315                         int y = (int)(p.y/p.z);
00316                         cvCircle(frame_undist, cvPoint(x,y), 6, CV_RGB(0,0,0), 2);
00317                         cvCircle(frame_undist, cvPoint(x,y), 2, CV_RGB(0,0,0), 2);
00318                         cvCircle(frame_undist, cvPoint(x,y), 4, CV_RGB(128,128,0), 3);
00319                     }
00320                 
00321                     cvCircle(frame_undist, X[0], 3, CV_RGB(255,255,128), 3);
00322                 }
00323 
00324                 cvShowImage("Calibration Result",frame_undist);
00325                 cvReleaseImage(&frame_undist);
00326             }
00327 
00328             if(allfound && ncorners == (int)_checkerboard.grid3d.size())
00329                 cvDrawChessboardCorners(frame, _checkerboard.griddims, &_checkerboard.corners[0], _checkerboard.corners.size(), 1);
00330             
00331             cvShowImage("Checkerboard Detector",frame);
00332         }
00333     }
00334 
00335     Transform FindTransformation(const vector<CvPoint2D32f> &imgpts, const vector<Vector> &objpts)
00336     {
00337         CvMat *objpoints = cvCreateMat(3,objpts.size(),CV_32FC1);
00338         for(size_t i=0; i<objpts.size(); ++i) {
00339             cvSetReal2D(objpoints, 0,i, objpts[i].x);
00340             cvSetReal2D(objpoints, 1,i, objpts[i].y);
00341             cvSetReal2D(objpoints, 2,i, objpts[i].z);
00342         }
00343         
00344         RaveTransform<float> pose;
00345         float fR3[3];
00346         CvMat R3, T3;
00347         cvInitMatHeader(&R3, 3, 1, CV_32FC1, fR3);
00348         cvInitMatHeader(&T3, 3, 1, CV_32FC1, &pose.trans.x);
00349 
00350         float kc[4] = {0};
00351         CvMat kcmat;
00352         cvInitMatHeader(&kcmat,1,4,CV_32FC1,kc);
00353 
00354         CvMat img_points;
00355         cvInitMatHeader(&img_points, 1,imgpts.size(), CV_32FC2, const_cast<CvPoint2D32f*>(&imgpts[0]));
00356     
00357         cvFindExtrinsicCameraParams2(objpoints, &img_points, _intrinsic_matrix, &kcmat, &R3, &T3);
00358         cvReleaseMat(&objpoints);
00359         
00360         double fang = sqrt(fR3[0]*fR3[0] + fR3[1]*fR3[1] + fR3[2]*fR3[2]);
00361         if( fang < 1e-6 ) {
00362             pose.rot.x = 1;
00363             pose.rot.y = 0;
00364             pose.rot.z = 0;
00365             pose.rot.w = 0;
00366         }
00367         else {
00368             double fmult = sin(fang/2)/fang;
00369             pose.rot.x = cos(fang/2);
00370             pose.rot.y = fR3[0]*fmult;
00371             pose.rot.z = fR3[1]*fmult;
00372             pose.rot.w = fR3[2]*fmult;
00373         }
00374 
00375         return pose;
00376     }
00377 
00378     static void MouseCallback(int event, int x, int y, int flags, void* param)
00379     {
00380         ((CheckerboardCalibration*)param)->_MouseCallback(event, x, y, flags);
00381     }
00382 
00383     void _MouseCallback(int event, int x, int y, int flags)
00384     {
00385         switch(event) {
00386         case CV_EVENT_RBUTTONDOWN:
00387             _bClearData = true;
00388             break;
00389         case CV_EVENT_MBUTTONDOWN:
00390             _bTakeObservation = true;
00391             break;
00392         }
00393     }
00394 };
00395 
00396 int main(int argc, char **argv)
00397 {
00398     ros::init(argc,argv,"checkerboard_calibration");
00399     boost::shared_ptr<CheckerboardCalibration> checker(new CheckerboardCalibration());
00400     
00401     ros::spin();
00402     checker.reset();
00403     return 0;
00404 }


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