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.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>
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;
00064 IplImage* frame;
00065 vector<CvPoint2D32f> _vAllPoints;
00066 vector<int> _vNumPointsPerImage;
00067
00068 CvMat *_intrinsic_matrix, *_distortion_coeffs;
00069 IplImage* _pUndistortionMapX, *_pUndistortionMapY;
00070 TransformMatrix _tProjection;
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
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
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
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
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
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 }
