00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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>
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;
00065 IplImage* frame;
00066 vector<CvPoint2D32f> _vAllPoints;
00067 vector<int> _vNumPointsPerImage;
00068
00069 CvMat *_intrinsic_matrix, *_distortion_coeffs;
00070 IplImage* _pUndistortionMapX, *_pUndistortionMapY;
00071 TransformMatrix _tProjection;
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
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
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
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
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
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 }