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
00027
00028
00029
00030
00031
00032
00033
00034
00041 #include <stdio.h>
00042 #include <string.h>
00043 #include <iostream>
00044 #include <unistd.h>
00045 #include <cstdlib>
00046
00047 #include <math.h>
00048 #include <opencv2/core/core.hpp>
00049 #include <opencv2/calib3d/calib3d.hpp>
00050 #include <opencv2/imgproc/imgproc.hpp>
00051 #include <opencv2/highgui/highgui.hpp>
00052
00053 #include <Eigen/Core>
00054 #include <Eigen/LU>
00055
00056 using namespace cv;
00057 using namespace Eigen;
00058 using namespace std;
00059
00060
00061 #define ROWS 480
00062 #define COLS 640
00063
00064
00065 cv::Point2f ir_depth_offset = cv::Point2f(-4, -3);
00066
00067 #define SHIFT_SCALE 0.125
00068
00069
00070 double shift2disp(int shift, double shift_offset)
00071 {
00072 return SHIFT_SCALE*(double)(shift_offset - shift);
00073 }
00074
00075
00076 uint16_t t_gamma[2048];
00077
00078 void setDepthColor(uint8_t *cptr, int d)
00079 {
00080 int pval = t_gamma[d];
00081 int lb = pval & 0xff;
00082 switch (pval >> 8)
00083 {
00084 case 0:
00085 cptr[2] = 255;
00086 cptr[1] = 255 - lb;
00087 cptr[0] = 255 - lb;
00088 break;
00089 case 1:
00090 cptr[2] = 255;
00091 cptr[1] = lb;
00092 cptr[0] = 0;
00093 break;
00094 case 2:
00095 cptr[2] = 255 - lb;
00096 cptr[1] = 255;
00097 cptr[0] = 0;
00098 break;
00099 case 3:
00100 cptr[2] = 0;
00101 cptr[1] = 255;
00102 cptr[0] = lb;
00103 break;
00104 case 4:
00105 cptr[2] = 0;
00106 cptr[1] = 255 - lb;
00107 cptr[0] = 255;
00108 break;
00109 case 5:
00110 cptr[2] = 0;
00111 cptr[1] = 0;
00112 cptr[0] = 255 - lb;
00113 break;
00114 default:
00115 cptr[2] = 0;
00116 cptr[1] = 0;
00117 cptr[0] = 0;
00118 break;
00119 }
00120 }
00121
00122 void writeCalibration(FILE *f, const cv::Mat& cameraMatrix, const cv::Mat& distCoeffs)
00123 {
00124 const double *K = cameraMatrix.ptr<double>();
00125 const double *D = distCoeffs.ptr<double>();
00126 fprintf(f, "image_width: 640\n");
00127 fprintf(f, "image_height: 480\n");
00128 fprintf(f, "camera_name: kinect\n");
00129 fprintf(f,
00130 "camera_matrix:\n"
00131 " rows: 3\n"
00132 " cols: 3\n"
00133 " data: [ %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f, %.8f ]\n",
00134 K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7], K[8]);
00135 fprintf(f,
00136 "distortion_coefficients:\n"
00137 " rows: 1\n"
00138 " cols: 5\n"
00139 " data: [ %.8f, %.8f, %.8f, %.8f, %.8f ]\n",
00140 D[0], D[1], D[2], D[3], D[4]);
00141 fprintf(f,
00142 "rectification_matrix:\n"
00143 " rows: 3\n"
00144 " cols: 3\n"
00145 " data: [ 1., 0., 0., 0., 1., 0., 0., 0., 1. ]\n");
00146 fprintf(f,
00147 "projection_matrix:\n"
00148 " rows: 3\n"
00149 " cols: 4\n"
00150 " data: [ %.8f, %.8f, %.8f, 0., %.8f, %.8f, %.8f, 0., %.8f, %.8f, %.8f, 0. ]\n",
00151 K[0], K[1], K[2], K[3], K[4], K[5], K[6], K[7], K[8]);
00152 }
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163 int
00164 main(int argc, char **argv)
00165 {
00166
00167 int ccols = 0;
00168 int crows = 0;
00169
00170
00171 double csize = 0.0;
00172
00173 char *fdir = NULL;
00174
00175 opterr = 0;
00176 int c;
00177 while ((c = getopt(argc, argv, "r:c:s:")) != -1)
00178 {
00179 switch (c)
00180 {
00181 case 'r':
00182 crows = atoi(optarg);
00183 break;
00184 case 'c':
00185 ccols = atoi(optarg);
00186 break;
00187 case 's':
00188 csize = atof(optarg);
00189 break;
00190 }
00191 }
00192
00193 if (optind < argc)
00194 fdir = argv[optind];
00195
00196 if (crows == 0 || ccols == 0 || csize == 0.0 || fdir == NULL)
00197 {
00198 printf("Must give the checkerboard dimensions and data directory.\n"
00199 "Usage:\n"
00200 "%s -r ROWS -c COLS -s SQUARE_SIZE my_data_dir\n", argv[0]);
00201 return 1;
00202 }
00203
00204
00205 vector<Point3f> pat;
00206 for (int i=0; i<ccols; i++)
00207 for (int j=0; j<crows; j++)
00208 pat.push_back(Point3f(i*csize,j*csize,0));
00209
00210
00211 vector<vector<Point3f> > pats;
00212 vector<vector<Point2f> > points;
00213
00214 int fnum = 0;
00215
00216 while (1)
00217 {
00218 char fname[1024];
00219 sprintf(fname,"%s/img_ir_%02d.png",fdir,fnum++);
00220 Mat img = imread(fname,-1);
00221 if (img.data == NULL) break;
00222
00223 vector<cv::Point2f> corners;
00224 bool ret = cv::findChessboardCorners(img,Size(crows,ccols),corners);
00225
00226 if (ret)
00227 printf("Found corners in image %s\n",fname);
00228 else {
00229 printf("*** Didn't find corners in image %s\n",fname);
00230 return 1;
00231 }
00232
00233 cv::cornerSubPix(img, corners, Size(5,5), Size(-1,-1),
00234 TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 0.1));
00235
00236
00237 for (int i = 0; i < corners.size(); ++i)
00238 corners[i] += ir_depth_offset;
00239
00240 pats.push_back(pat);
00241 points.push_back(corners);
00242 }
00243
00244
00245
00246 Mat camMatrix;
00247 Mat distCoeffs;
00248 vector<Mat> rvecs;
00249 vector<Mat> tvecs;
00250 double rp_err;
00251
00252 rp_err = calibrateCamera(pats, points, Size(COLS,ROWS), camMatrix, distCoeffs,
00253 rvecs, tvecs,
00254 CV_CALIB_FIX_K3 |
00255 CV_CALIB_FIX_K2 |
00256 CV_CALIB_FIX_K1 |
00257 CV_CALIB_ZERO_TANGENT_DIST |
00258
00259 CV_CALIB_FIX_ASPECT_RATIO
00260 );
00261
00262 printf("\nCalibration results:\n");
00263
00264
00265 printf("\nCamera matrix\n");
00266 double *dptr = camMatrix.ptr<double>(0);
00267 for (int i=0; i<3; i++)
00268 {
00269 for (int j=0; j<3; j++)
00270 printf("%f ",*dptr++);
00271 printf("\n");
00272 }
00273
00274 dptr = distCoeffs.ptr<double>(0);
00275 printf("\nDistortion coefficients:\n"
00276 "k1: %f\n"
00277 "k2: %f\n"
00278 "t1: %f\n"
00279 "t2: %f\n"
00280 "k3: %f\n", dptr[0], dptr[1], dptr[2], dptr[3], dptr[4]);
00281
00282 printf("\nReprojection error = %f\n\n", rp_err);
00283
00284 char depth_fname[1024];
00285 sprintf(depth_fname, "%s/calibration_depth.yaml", fdir);
00286 FILE *depth_file = fopen(depth_fname, "w");
00287 if (depth_file) {
00288 writeCalibration(depth_file, camMatrix, distCoeffs);
00289 printf("Wrote depth camera calibration to %s\n\n", depth_fname);
00290 }
00291
00292
00294 fnum = 0;
00295 std::vector<cv::Vec2d> ls_src1;
00296 std::vector<double> ls_src2;
00297
00298 while (1)
00299 {
00300
00301 char fname[1024];
00302 sprintf(fname,"%s/img_depth_%02d.png",fdir,fnum);
00303 Mat img_depth = imread(fname,-1);
00304 if (img_depth.data == NULL) break;
00305
00306
00307 const cv::Mat pattern(pats[fnum]);
00308 vector<Point2f> &corners = points[fnum];
00309 cv::Mat rvec = rvecs[fnum];
00310 cv::Mat tvec = tvecs[fnum];
00311 cv::Mat rot3x3;
00312 cv::Rodrigues(rvec, rot3x3);
00313
00314
00315 cv::Mat world_points;
00316 cv::Mat xfm(3, 4, cv::DataType<double>::type);
00317 cv::Mat xfm_rot = xfm.colRange(0,3);
00318 cv::Mat xfm_trans = xfm.col(3);
00319 rot3x3.copyTo(xfm_rot);
00320 tvec.reshape(1,3).copyTo(xfm_trans);
00321 cv::transform(pattern, world_points, xfm);
00322
00323 for (int j = 0; j < corners.size(); ++j) {
00324 double Z = world_points.at<cv::Vec3f>(j)[2];
00325 double r = img_depth.at<uint16_t>(corners[j]);
00326 ls_src1.push_back(cv::Vec2d(-1.0, Z));
00327 ls_src2.push_back(Z*r);
00328
00329 }
00330
00331 fnum++;
00332 }
00333
00334 cv::Mat depth_params;
00335 double A, B;
00336 double b;
00337 if (cv::solve(cv::Mat(ls_src1).reshape(1), cv::Mat(ls_src2), depth_params,
00338 DECOMP_LU | DECOMP_NORMAL)) {
00339 A = depth_params.at<double>(0);
00340 B = depth_params.at<double>(1);
00341 double f = camMatrix.ptr<double>()[0];
00342 b = SHIFT_SCALE * A / f;
00343 printf("Reading to depth fitting parameters:\n"
00344 "A = %f\n"
00345 "B = %f\n"
00346 "Baseline between projector and depth camera = %f\n",
00347 A, B, b);
00348 }
00349 else {
00350 printf("**** Failed to solve least-squared problem ****\n");
00351 return 1;
00352 }
00353
00354
00355
00356
00357
00358
00359 fnum = 0;
00360 vector<vector<Point2f> > pointsRGB;
00361 printf("\n");
00362 while (1)
00363 {
00364 char fname[1024];
00365 sprintf(fname,"%s/img_rgb_%02d.png",fdir,fnum);
00366 Mat img = imread(fname,1);
00367 if (img.data == NULL) break;
00368
00369 vector<cv::Point2f> corners;
00370 bool ret = cv::findChessboardCorners(img,Size(crows,ccols),corners);
00371
00372 if (ret)
00373 printf("Found corners in image %s\n",fname);
00374 else {
00375 printf("*** Didn't find corners in image %s\n",fname);
00376 return 1;
00377 }
00378
00379 Mat gray;
00380 cv::cvtColor(img, gray, CV_RGB2GRAY);
00381 cv::cornerSubPix(gray, corners, Size(5,5), Size(-1,-1),
00382 TermCriteria(TermCriteria::MAX_ITER+TermCriteria::EPS, 30, 0.1));
00383
00384 pointsRGB.push_back(corners);
00385
00386 fnum++;
00387 }
00388
00389
00390 Mat camMatrixRGB;
00391 Mat distCoeffsRGB = Mat::zeros(5,1,CV_64F);
00392
00393 camMatrixRGB = (Mat_<double>(3,3) << 1, 0, 320, 0, 1, 240, 0, 0, 1);
00394
00395 rp_err = calibrateCamera(pats, pointsRGB, Size(COLS,ROWS), camMatrixRGB, distCoeffsRGB,
00396 rvecs, tvecs,
00397
00398
00399 CV_CALIB_FIX_K3 |
00400 CV_CALIB_ZERO_TANGENT_DIST |
00401
00402 CV_CALIB_FIX_ASPECT_RATIO
00403 );
00404
00405
00406 printf("\nCalibration results:\n");
00407
00408
00409 printf("\nCamera matrix\n");
00410 dptr = camMatrixRGB.ptr<double>(0);
00411 for (int i=0; i<3; i++)
00412 {
00413 for (int j=0; j<3; j++)
00414 printf("%f ",*dptr++);
00415 printf("\n");
00416 }
00417
00418 dptr = distCoeffsRGB.ptr<double>(0);
00419 printf("\nDistortion coefficients:\n"
00420 "k1: %f\n"
00421 "k2: %f\n"
00422 "t1: %f\n"
00423 "t2: %f\n"
00424 "k3: %f\n", dptr[0], dptr[1], dptr[2], dptr[3], dptr[4]);
00425
00426 printf("\nReprojection error = %f\n\n", rp_err);
00427
00428 char rgb_fname[1024];
00429 sprintf(rgb_fname, "%s/calibration_rgb.yaml", fdir);
00430 FILE *rgb_file = fopen(rgb_fname, "w");
00431 if (rgb_file) {
00432 writeCalibration(rgb_file, camMatrixRGB, distCoeffsRGB);
00433 printf("Wrote RGB camera calibration to %s\n\n", rgb_fname);
00434 }
00435
00436
00437 Mat R,T,E,F;
00438 rp_err = stereoCalibrate(pats,points,pointsRGB,camMatrix,distCoeffs,
00439 camMatrixRGB,distCoeffsRGB,Size(crows,ccols),
00440 R,T,E,F);
00441
00442 dptr = T.ptr<double>(0);
00443 printf("\nTranslation between depth and RGB sensors (m):\n");
00444 for (int i=0; i<3; i++)
00445 printf("%f ",dptr[i]);
00446 printf("\n");
00447
00448 printf("\nRotation matrix:\n");
00449 dptr = R.ptr<double>(0);
00450 for (int i=0; i<3; i++)
00451 {
00452 for (int j=0; j<3; j++)
00453 printf("%f ",*dptr++);
00454 printf("\n");
00455 }
00456 printf("\nReprojection error = %f\n\n", rp_err);
00457
00458
00459 Matrix4d Q,S;
00460 Matrix<double,3,4> P;
00461
00462
00463 double *cptr = camMatrix.ptr<double>(0);
00464 Q << 1, 0, 0, -cptr[2],
00465 0, 1, 0, -cptr[5],
00466 0, 0, 0, cptr[0],
00467 0, 0, 1.0/b, 0;
00468
00469
00470 dptr = R.ptr<double>(0);
00471 double *tptr = T.ptr<double>(0);
00472 S << dptr[0], dptr[1], dptr[2], tptr[0],
00473 dptr[3], dptr[4], dptr[5], tptr[1],
00474 dptr[6], dptr[7], dptr[8], tptr[2],
00475 0, 0, 0, 1;
00476
00477
00478 cptr = camMatrixRGB.ptr<double>(0);
00479 P << cptr[0], 0, cptr[2], 0,
00480 0, cptr[4], cptr[5], 0,
00481 0, 0, 1, 0;
00482
00483 Matrix<double,3,4> D = P*S*Q;
00484 std::cout << "Transform matrix:" << std::endl << D << std::endl << std::endl;
00485
00486 char params_fname[1024];
00487 sprintf(params_fname, "%s/kinect_params.yaml", fdir);
00488 FILE *params_file = fopen(params_fname, "w");
00489 if (params_file) {
00490 fprintf(params_file, "shift_offset: %.4f\n", B);
00491 fprintf(params_file, "projector_depth_baseline: %.5f\n", b);
00492 dptr = R.ptr<double>(0);
00493 fprintf(params_file,
00494 "depth_rgb_rotation: [ %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ]\n",
00495 dptr[0], dptr[1], dptr[2], dptr[3], dptr[4], dptr[5], dptr[6], dptr[7], dptr[8]);
00496 dptr = T.ptr<double>(0);
00497 fprintf(params_file,
00498 "depth_rgb_translation: [ %.6f, %.6f, %.6f ]\n", dptr[0], dptr[1], dptr[2]);
00499 printf("Wrote additional calibration parameters to %s\n", params_fname);
00500 }
00501
00502
00503
00504
00505
00506
00507 for (int i = 0; i < 2048; i++)
00508 {
00509 float v = i / 2048.0;
00510 v = powf (v, 3) * 6;
00511 t_gamma[i] = v * 6 * 256;
00512 }
00513
00514
00515 fnum = 0;
00516 printf("Creating output images\n");
00517 while (1)
00518 {
00519 char fname[1024];
00520 sprintf(fname,"%s/img_depth_%02d.png",fdir,fnum);
00521 Mat img = imread(fname,-1);
00522 if (img.data == NULL) break;
00523
00524 sprintf(fname,"%s/img_rgb_%02d.png",fdir,fnum);
00525 Mat imgRGB = imread(fname,1);
00526 if (imgRGB.data == NULL) break;
00527
00528
00529 cv::Mat imgRgbRect;
00530 cv::undistort(imgRGB, imgRgbRect, camMatrixRGB, distCoeffsRGB);
00531
00532 uint16_t *dptr = img.ptr<uint16_t>(0);
00533
00534 Mat imgr = Mat::zeros(ROWS,COLS,CV_16UC1);
00535 Mat imgrc = Mat::zeros(ROWS,COLS,CV_8UC3);
00536 Mat imgc = Mat::zeros(ROWS,COLS,CV_8UC3);
00537 Mat imgdc = Mat::zeros(ROWS,COLS,CV_8UC3);
00538
00539 uint16_t *rptr = imgr.ptr<uint16_t>(0);
00540 uint8_t *rcptr = imgrc.ptr<uint8_t>(0);
00541 uint8_t *cptr = imgc.ptr<uint8_t>(0);
00542 uint8_t *dcptr = imgdc.ptr<uint8_t>(0);
00543 uint8_t *rgbptr = imgRgbRect.ptr<uint8_t>(0);
00544
00545 int k=0;
00546 for (int i=0; i<ROWS; i++)
00547 for (int j=0; j<COLS; j++,k++)
00548 {
00549 double d = shift2disp(dptr[k], B);
00550 if (d <= 0)
00551 d = 0.0;
00552 Vector4d p;
00553 p << j,i,d,1;
00554 Vector3d q;
00555 q = D*p;
00556 int u = (int)(q[0]/q[2]+0.5);
00557 int v = (int)(q[1]/q[2]+0.5);
00558 setDepthColor(&cptr[3*(i*COLS+j)],dptr[k]);
00559 if (u < 0 || v < 0 || u >= COLS || v >= ROWS)
00560 continue;
00561 int disp = (int)(d*16+0.499);
00562 int kk = v*COLS+u;
00563 if (rptr[kk] < disp)
00564 {
00565 rptr[kk] = disp;
00566 setDepthColor(&rcptr[3*kk],dptr[k]);
00567 }
00568 if (d != 0.0)
00569 memcpy(&dcptr[3*k],&rgbptr[3*kk],3);
00570 }
00571
00572 sprintf(fname,"%s/img_depth_rect_%02d.png",fdir,fnum);
00573 printf("Writing %s\n", fname);
00574 imwrite(fname,imgr);
00575 sprintf(fname,"%s/img_depth_rect_color_%02d.png",fdir,fnum);
00576 printf("Writing %s\n", fname);
00577 imwrite(fname,imgrc);
00578 sprintf(fname,"%s/img_depth_color_%02d.png",fdir,fnum);
00579 printf("Writing %s\n", fname);
00580 imwrite(fname,imgc);
00581 sprintf(fname,"%s/img_rgb_mapped_%02d.png",fdir,fnum);
00582 printf("Writing %s\n", fname);
00583 imwrite(fname,imgdc);
00584 sprintf(fname,"%s/img_rgb_rect_%02d.png",fdir,fnum);
00585 printf("Writing %s\n", fname);
00586 imwrite(fname,imgRgbRect);
00587
00588 fnum++;
00589 }
00590
00591 return 0;
00592 }