00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00048 #include <iostream>
00049 #include <fstream>
00050
00051 #include <QVMatrix>
00052 #include <QVApplication>
00053 #include <QVImageCanvas>
00054 #include <QVMPlayerReaderBlock>
00055 #include <QVDefaultGUI>
00056
00057 #include <qvip.h>
00058 #include <qvimageio.h>
00059 #include <qvprojective.h>
00060
00061 #ifndef DOXYGEN_IGNORE_THIS
00062 #ifdef OPENCV
00063 #include <cv.h>
00064 #include <highgui.h>
00065
00066 #define NUM_CHESSBOARDS 16
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110 double error(const double u, const double v, const double x, const double y, const QVVector k)
00111 {
00112 const double r2 = x*x+y*y, r4 = r2*r2, r6 = r4*r2, r8 = r4*r4,
00113 d = 1 + k[0] * r2 + k[1] * r4 + k[2] * r6 + k[3] * r8;
00114 const double errorU = u - x * d, errorV = v - y * d;
00115
00116 return errorU * errorU + errorV * errorV;
00117 }
00118
00119 QPointF findMinimum(const QPointF &orig, const QVVector k)
00120 {
00121 const int u = orig.x(), v = orig.y();
00122 QPointF actual = orig;
00123
00124 while(true)
00125 {
00126 QPointF best = actual;
00127 double bestError = std::numeric_limits<double>::max();
00128
00129 for (double i = -0.01; i <= 0.01; i+=0.0025)
00130 for (double j = -0.01; j <= 0.01; j+=0.0025)
00131 if (error(u, v, actual.x()+i, actual.y()+j, k) < bestError)
00132 best = actual + QPoint(i, j);
00133 if (actual == best)
00134 break;
00135
00136 actual = best;
00137 }
00138 return actual;
00139 }
00140
00141 class CameraCalibration
00142 {
00143 public:
00144 int cols, rows;
00145 QVMatrix K, distortionCoeffs;
00146
00147 QVMatrix Kinv;
00148 IplImage *dx, *dy;
00149
00150 bool initCacheMap(IplImage *&dx_, IplImage *&dy_, const QVMatrix &A2 = QVMatrix::identity(3))
00151 {
00152 Kinv = pseudoInverse(K);
00153
00154 CvMat * camera_matrix = K.toCvMat(CV_32F),
00155 * dist_coeffs = distortionCoeffs.toCvMat(CV_32FC1),
00156 * new_camera_matrix = A2.toCvMat(CV_32F);
00157
00158 if (dx_ != NULL)
00159 cvReleaseImage(&dx);
00160 if (dy_ != NULL)
00161 cvReleaseImage(&dy);
00162
00163 if (cols == 0 or rows == 0)
00164 return false;
00165
00166 dx_ = cvCreateImage(cvSize(cols, rows), IPL_DEPTH_32F, 1);
00167 dy_ = cvCreateImage(cvSize(cols, rows), IPL_DEPTH_32F, 1);
00168
00169 cvInitUndistortRectifyMap(camera_matrix, dist_coeffs, NULL , new_camera_matrix, dx_, dy_);
00170
00171 cvReleaseMat(&camera_matrix);
00172 cvReleaseMat(&dist_coeffs);
00173 cvReleaseMat(&new_camera_matrix);
00174
00175 return true;
00176 }
00177
00178 public:
00179 CameraCalibration(const CameraCalibration &other):
00180 cols(other.cols), rows(other.rows), K(other.K), distortionCoeffs(other.distortionCoeffs), dx(NULL), dy(NULL)
00181 {
00182 initCacheMap(dx, dy);
00183 }
00184
00185 CameraCalibration( const int cols = 0, const int rows = 0,
00186 const QVMatrix &K = QVMatrix::identity(3),
00187 const QVMatrix &distortionCoeffs = QVMatrix(4, 1, 0.0)):
00188 cols(cols), rows(rows), K(K), distortionCoeffs(distortionCoeffs), dx(NULL), dy(NULL)
00189 {
00190 initCacheMap(dx,dy);
00191 }
00192
00193 ~CameraCalibration()
00194 {
00195 if (dx != NULL)
00196 cvReleaseImage(&dx);
00197 if (dy != NULL)
00198 cvReleaseImage(&dy);
00199 }
00200
00201 CameraCalibration & operator = (const CameraCalibration &other)
00202 {
00203 cols = other.cols;
00204 rows = other.rows;
00205 K = other.K;
00206 distortionCoeffs = other.distortionCoeffs;
00207 initCacheMap(dx,dy);
00208 return (*this);
00209 }
00210
00211 bool inited() const
00212 {
00213 return (cols != 0 and rows != 0);
00214 }
00215
00216 bool loadFromFile(const QString &fileName)
00217 {
00218 std::ifstream stream;
00219 stream.open(qPrintable(fileName));
00220
00221 if ( stream.fail() )
00222 return false;
00223
00224 stream >> cols;
00225 stream >> rows;
00226 stream >> K;
00227 stream >> distortionCoeffs;
00228
00229 stream.close();
00230
00231 return initCacheMap(dx,dy);
00232 }
00233
00234 bool saveToFile(const QString &fileName) const
00235 {
00236 std::ofstream stream;
00237 stream.open(qPrintable(fileName));
00238
00239 if ( stream.fail() )
00240 return false;
00241
00242 stream << cols << std::endl;
00243 stream << rows << std::endl;
00244 stream << K;
00245 stream << distortionCoeffs;
00246
00247 stream.close();
00248
00249 return true;
00250 }
00251
00252 QPointF map(const int i, const int j) const
00253 {
00254
00255 return QPointF(CV_IMAGE_ELEM(dx, float, j, i), CV_IMAGE_ELEM(dy, float, j, i));
00256 }
00257
00258 QVImage<uChar, 1> radialUndistort(const QVImage<uChar, 1> &image)
00259 {
00260 if (cols == 0 or rows == 0)
00261 return QVImage<uChar, 1>();
00262
00263
00264 QVMatrix A2 = K;
00265 A2(0,0) = 0.75*A2(0,0);
00266 A2(1,1) = 0.75*A2(1,1);
00267 A2(0,2) = 160.0;
00268 A2(1,2) = 100.0;
00269
00270 IplImage *dx_ = NULL, *dy_ = NULL;
00271
00272 initCacheMap(dx_, dy_, A2);
00273
00274
00275 IplImage *src = image, *dst = image;
00276
00277 cvRemap(src, dst, dx_, dy_, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
00278 QVImage<uChar, 1> outputImage = QVImage<uChar, 1>(dst);
00279
00280 cvReleaseImage(&dx_);
00281 cvReleaseImage(&dy_);
00282 cvReleaseImage(&src);
00283 cvReleaseImage(&dst);
00284
00285 return outputImage;
00286 }
00287 };
00288
00289 QHash<QV3DPointF, QPointF> detectBoard(const QVImage<uChar, 3> &actualImage, const int board_w = 6, const int board_h = 9)
00290 {
00291 const int board_n = board_w * board_h;
00292 const CvSize board_sz = cvSize(board_w, board_h);
00293 CvPoint2D32f* corners = new CvPoint2D32f[ board_n ];
00294
00295 IplImage *image = actualImage;
00296 IplImage *gray_image = cvCreateImage(cvGetSize(image), 8, 1);
00297
00298 CvSize image_sz = cvGetSize(image);
00299
00300 int corner_count, found = cvFindChessboardCorners(
00301 image, board_sz, corners, &corner_count,
00302 CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS);
00303
00304
00305 cvCvtColor(image, gray_image, CV_BGR2GRAY);
00306 cvFindCornerSubPix( gray_image, corners, corner_count, cvSize(11,11),cvSize(-1,-1),
00307 cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER, 30, 0.1));
00308
00309 cvReleaseImage(&image);
00310 cvReleaseImage(&gray_image);
00311
00312 QHash<QV3DPointF, QPointF> boardCorners;
00313 if(corner_count == board_n)
00314 for(int j=0; j<board_n; ++j)
00315 boardCorners[QV3DPointF(j/board_w, j%board_w, 0.0f)] = QPointF(corners[j].x, corners[j].y);
00316
00317 delete corners;
00318 return boardCorners;
00319 }
00320
00321 CameraCalibration calibrateIntrinsicsFrom3DTo2DCorrespondences(const QList< QHash<QV3DPointF, QPointF> > &pointCorrespondences, const int cols, const int rows)
00322 {
00323
00324 int num_points = 0;
00325 for(int i = 0, index = 0; i < pointCorrespondences.size(); i++)
00326 num_points += pointCorrespondences[index].count();
00327
00328
00329 CvMat* object_points = cvCreateMat(num_points,3,CV_32FC1);
00330 CvMat* image_points = cvCreateMat(num_points,2,CV_32FC1);
00331 CvMat* point_counts = cvCreateMat(pointCorrespondences.size(),1,CV_32SC1);
00332
00333
00334 for(int i = 0, index = 0; i < pointCorrespondences.size(); i++)
00335 {
00336 foreach(QV3DPointF point3D, pointCorrespondences[i].keys())
00337 {
00338 CV_MAT_ELEM( *image_points, float, index, 0) = pointCorrespondences[i][point3D].x();
00339 CV_MAT_ELEM( *image_points, float, index, 1) = pointCorrespondences[i][point3D].y();
00340 CV_MAT_ELEM( *object_points, float, index, 0) = point3D.x();
00341 CV_MAT_ELEM( *object_points, float, index, 1) = point3D.y();
00342 CV_MAT_ELEM( *object_points, float, index, 2) = point3D.z();
00343
00344 index++;
00345 }
00346 CV_MAT_ELEM(*point_counts, int, i, 0) = pointCorrespondences[i].count();
00347 }
00348
00349
00350 CvMat* intrinsic_matrix = cvCreateMat(3,3,CV_32FC1);
00351 CvMat* distortion_coeffs = cvCreateMat(4,1,CV_32FC1);
00352
00353 CV_MAT_ELEM( *intrinsic_matrix, float, 0, 0 ) = 1.0f;
00354 CV_MAT_ELEM( *intrinsic_matrix, float, 1, 1 ) = 1.0f;
00355
00356
00357 cvCalibrateCamera2(object_points, image_points, point_counts, cvSize(cols,rows), intrinsic_matrix, distortion_coeffs, NULL, NULL,0);
00358
00359
00360 CameraCalibration result = CameraCalibration(cols, rows, intrinsic_matrix, distortion_coeffs);
00361
00362
00363 cvReleaseMat(&object_points);
00364 cvReleaseMat(&image_points);
00365 cvReleaseMat(&point_counts);
00366 cvReleaseMat(&intrinsic_matrix);
00367 cvReleaseMat(&distortion_coeffs);
00368
00369 return result;
00370 }
00371
00372 class CameraCalibratorBlock: public QVProcessingBlock
00373 {
00374 private:
00375 QVImage<uChar, 3> actualImage;
00376
00377 QList< QHash<QV3DPointF, QPointF> > grabbedBoards;
00378
00379 CameraCalibration calibration;
00380
00381 public:
00382 CameraCalibratorBlock(QString name): QVProcessingBlock(name)
00383 {
00384 addProperty< QVImage<uChar,3> >("Input image", inputFlag|outputFlag);
00385 addProperty< QVImage<uChar,3> >("Output image", outputFlag);
00386 addProperty< QString >("Camera file name", inputFlag|outputFlag, "calibrated.camera");
00387
00388 addProperty< QList<QPointF> >("Board corners", outputFlag);
00389 addTrigger("Grab chessboard");
00390 addTrigger("Calibrate");
00391
00392 if (calibration.loadFromFile(getPropertyValue< QString >("Camera file name")))
00393 std::cout << "Camera loaded" << std::endl;
00394 else
00395 std::cout << "Could not load camera" << std::endl;
00396 }
00397
00398 void processTrigger(const QString triggerName)
00399 {
00400 if (triggerName == "Grab chessboard")
00401 {
00402 const QHash<QV3DPointF, QPointF> grabbedBoard = detectBoard(actualImage);
00403 if (grabbedBoard.count() > 0)
00404 {
00405 grabbedBoards << grabbedBoard;
00406 writeQVImageToFile( QString("temp/frame-") + QString::number(getIteration()).QString::rightJustified(8, '0') + ".png",
00407 actualImage);
00408 }
00409 std::cout << "Captured " << grabbedBoards.size() << " boards"<< std::endl;
00410 }
00411 else if (triggerName == "Calibrate")
00412 {
00413 std::cout << "Proceeding to camera calibration." << std::endl;
00414 calibration = calibrateIntrinsicsFrom3DTo2DCorrespondences(grabbedBoards, 320, 240);
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433 if (calibration.saveToFile("calibrated.camera"))
00434 std::cout << "Camera calibrated and stored." << std::endl;
00435 else
00436 std::cout << "Error storing camera." << std::endl;
00437 }
00438 std::cout << "End of trigger processing" << std::endl;
00439 }
00440
00441 void iterate()
00442 {
00443 actualImage = getPropertyValue<QVImage<uChar, 3> >("Input image");
00444
00445 const QHash<QV3DPointF, QPointF> boardCorners = detectBoard(actualImage);
00446 setPropertyValue< QList<QPointF> >("Board corners", boardCorners.values());
00447
00448 if (not calibration.inited())
00449 return;
00450
00451 if (boardCorners.count() > 0)
00452 {
00453 QList<QPointFMatching> matchings;
00454 foreach(QV3DPointF point3D, boardCorners.keys())
00455 {
00456 const QPointF p = boardCorners[point3D];
00457 matchings << QPointFMatching(QPointF(point3D.x(), point3D.y()), ApplyHomography(calibration.Kinv,p));
00458 }
00459 const QVMatrix H = ComputeProjectiveHomography(matchings),
00460 errorM = H.transpose()*H,
00461 normalizedErrorM = errorM * (2.0 / (errorM(0,0) + errorM(1,1)));
00462
00463 const double a = errorM(0,0), b = errorM(0,1), c = errorM(1,0), d = errorM(1,1);
00464
00465 std::cout << ( ABS(1-normalizedErrorM(0,0)) + ABS(1- normalizedErrorM(1,1)) + 2*ABS(normalizedErrorM(1,0)) ) << std::endl;
00466 std::cout << "H^t * H = " << normalizedErrorM << std::endl;
00467 }
00468
00469
00470 }
00471 };
00472
00473 #include <QVVector>
00474 #include <QVMatrix>
00475 int main(int argc, char *argv[])
00476 {
00477 QVApplication app(argc, argv, "Example program for QVision library. Displays the contents of a video source.");
00478
00479 QVMPlayerReaderBlock camera("Video");
00480
00481 CameraCalibratorBlock player("Video player");
00482 camera.linkProperty(&player,"Input image");
00483
00484 QVDefaultGUI interface;
00485
00486 QVImageCanvas inputImageCanvas("Input image");
00487 player.linkProperty("Input image", inputImageCanvas);
00488 player.linkProperty("Board corners", inputImageCanvas);
00489
00490 QVImageCanvas outputImageCanvas("Output image");
00491 player.linkProperty("Output image", outputImageCanvas);
00492
00493
00494 return app.exec();
00495 }
00496 #else // not OPENCV
00497 int main(int argc, char* argv[])
00498 {
00499 std::cout << "ERROR: OpenCV compatibility was not activated in QVision compilation." << std::endl;
00500 return -1;
00501 }
00502 #endif // OPENCV
00503 #endif // DOXYGEN_IGNORE_THIS
00504