examples/cameraCalibrator/cameraCalibrator.cpp

00001 /*
00002  *      Copyright (C) 2007. PARP Research Group.
00003  *      <http://perception.inf.um.es>
00004  *      University of Murcia, Spain.
00005  *
00006  *      This file is part of the QVision library.
00007  *
00008  *      QVision is free software: you can redistribute it and/or modify
00009  *      it under the terms of the GNU Lesser General Public License as
00010  *      published by the Free Software Foundation, version 3 of the License.
00011  *
00012  *      QVision is distributed in the hope that it will be useful,
00013  *      but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *      GNU Lesser General Public License for more details.
00016  *
00017  *      You should have received a copy of the GNU Lesser General Public
00018  *      License along with QVision. If not, see <http://www.gnu.org/licenses/>.
00019  */
00020 
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <iostream>
00041 #include <fstream>
00042 #include <math.h>
00043 
00044 #include <QDebug>
00045 #include <QThread>
00046 #include <QSet>
00047 
00048 #include <QVApplication>
00049 #include <QVMPlayerCamera>
00050 #include <QVGUI>
00051 #include <QVImageCanvas>
00052 #include <QVGLCanvas>
00053 
00054 #include <QVDisjointSet>
00055 #include <QV3DModel>
00056 #include <QVMatrix>
00057 #include <QVRANSAC>
00058 #include <QVPlanarRectifierWorker>
00059 
00060 #include <qvdta/qvdta.h>
00061 #include <qvip/qvip.h>
00062 #include <qvmath/qvprojective.h>
00063 
00064 #include <planartemplatedetector.h>
00065 
00066 #ifndef DOXYGEN_IGNORE_THIS
00067 
00068 #include <QVMultidimensionalMinimizer>
00069 class CameraMinimizator: public QVMultidimensionalMinimizer
00070         {
00071         private:
00072                 const QSet<QVMatrix> &homographies;
00073 
00074                 const QVMatrix cameraMatrix(const QVVector &vector) const
00075                         {
00076                         QVMatrix K(3,3);
00077                         K.set(0);
00078                         K(0,0) = vector[0];
00079                         K(1,1) = vector[1];
00080                         K(0,2) = vector[2];
00081                         K(1,2) = vector[3];
00082                         K(0,1) = vector[4];
00083                         K(2,2) = 1;
00084 
00085                         return K;
00086                         }
00087 
00088         public:
00089                 CameraMinimizator(const QSet<QVMatrix> &homographies, const double maxStep = 1e-6): QVMultidimensionalMinimizer(5, maxStep),
00090                         homographies(homographies)
00091                                 { }
00092 
00093                 double function (const QVVector &v)
00094                         {
00095                         const QVMatrix  K = cameraMatrix(v), omega = pseudoInverse(K.transpose()) * pseudoInverse(K);
00096 
00097                         double accum = 0;
00098                         foreach (QVMatrix H, homographies)
00099                                 {
00100                                 const QVMatrix  error = H.transpose() * omega * H;
00101                                 accum += POW2(error(0,1)/error(0,0)) + POW2(error(1,1) / error(0,0) -1);
00102                                 }
00103 
00104                         return accum;
00105                         }
00106 
00107                 const QVMatrix getHomography() const    { return cameraMatrix(getMinimum()); }
00108         };
00109 
00110 class CameraCalibrator: public QVWorker
00111         {
00112         private:
00113                 QSet<QVMatrix> matrixSet;
00114                 QVMatrix lastMatrix;
00115 
00116         public:
00117                 CameraCalibrator(QString name): QVWorker(name), lastMatrix(QVMatrix::identity(3))
00118                         {
00119                         addProperty< QVMatrix >("Homography", inputFlag);
00120                         addProperty<double>("Matrix dist", inputFlag, 1.0, "--", 0.01, 10.0);
00121                         addProperty<double>("Max error", inputFlag, -6, "--", -10, -1);
00122                         addProperty<double>("Max step", inputFlag, -6, "--", -10, -1);
00123                         addProperty<int>("Max iterations", inputFlag, 1000, "--", 100, 10000);
00124                         addTrigger("Calibrate");
00125 
00126                         addProperty<QString>("CameraMatrixFile", inputFlag, "cameraMatrix.txt", "Path to the file to store camera matrix");
00127                         }
00128 
00129                 void processTrigger(QString triggerName)
00130                         {
00131                         const double maxError = pow(10,getPropertyValue< double >("Max error")),
00132                                 step = pow(10,getPropertyValue< double >("Max step"));
00133                         const int maxIterations = getPropertyValue< int >("Max iterations");
00134 
00135                         std::cout << "Set size = " << matrixSet.size() << std::endl;
00136                         CameraMinimizator minimizator(matrixSet , step);
00137 
00138                         QVVector firstPoint(5), lastPoint(5);
00139                         firstPoint[0] = firstPoint[1] = +1.0;   firstPoint[2] = firstPoint[3] = -2.5; firstPoint[4] = -1;
00140                         lastPoint[0] = lastPoint[1] = +5.0;     lastPoint[2] = lastPoint[3] = +2.5; lastPoint[4] = 1;
00141 
00142                         const QList<QVVector> minimums = minimizator.gridIterate(firstPoint, lastPoint, 3, maxIterations, maxError);
00143 
00144                         if (minimums.size() > 0)
00145                                 {
00146                                 std::cout << "K = " << minimizator.getHomography() << std::endl;
00147                                 std::ofstream kfile;
00148                                 kfile.open (qPrintable(getPropertyValue<QString>("CameraMatrixFile")));
00149                                 kfile << minimizator.getHomography();
00150                                 kfile.close();
00151                                 }
00152                         else
00153                                 std::cout << "No minimum obtained." << std::endl;
00154                         };
00155 
00156                 void iterate()
00157                         {
00158                         // 0. Read input property values.
00159                         const QVMatrix H = getPropertyValue< QVMatrix >("Homography");
00160 
00161                         if (H.getCols() != 3 || H.getRows() != 3)
00162                                 return;
00163 
00164                         const double minDist = getPropertyValue< double >("Matrix dist");
00165 
00166                         if ( (H-lastMatrix).norm2() > minDist)
00167                                 if (!matrixSet.contains(H))
00168                                         matrixSet.insert(pseudoInverse(lastMatrix = H));
00169 
00170                         QVMatrix M(6,5);
00171                         for (int i = 0; i < M.getRows(); i++)
00172                                 for (int j = 0; j < M.getCols(); j++)
00173                                         M(i,j) = j;
00174                         QVVector x(10);
00175                         solveHomogeneousLinear(M, x);
00176 
00177                         // 1. Store and show results.
00178                         timeFlag("Image wrapping");
00179                         }
00180         };
00181 
00183 
00184 int main(int argc, char *argv[])
00185         {
00186         QVApplication app(argc, argv,
00187                 "Example program for QVision library. Obtains intrinsic and extrinsic camera parameters."
00188                 );
00189 
00190         QVMPlayerCamera camera("Video");
00191 
00192         // Camera calibrator.
00193         PlanarTemplateDetector planarTemplateDetector("Planar template detector", "template3d.gif");
00194         camera.link(&planarTemplateDetector,"Input image");
00195 
00196         CameraCalibrator cameraCalibrator("Calibrator worker");
00197         planarTemplateDetector.linkProperty("Homography", &cameraCalibrator, "Homography", QVWorker::SynchronousLink);
00198 
00199         // Image warper.
00200         QVPlanarRectifierWorker warper("Image warper");
00201         planarTemplateDetector.linkProperty("Input image", &warper, "Input image", QVWorker::SynchronousLink);
00202         planarTemplateDetector.linkProperty("Homography", &warper, "Homography", QVWorker::SynchronousLink);
00203 
00204         // Image outputs
00205         QVGUI interface;
00206 
00207         QVImageCanvas imageCanvas("Corners ");
00208         imageCanvas.linkProperty(planarTemplateDetector, "Input image");
00209         imageCanvas.linkProperty(planarTemplateDetector,"Corners", Qt::red, true);
00210 
00211         QVImageCanvas imageCanvas2("Warped");
00212         imageCanvas2.linkProperty(warper, "Warped image");
00213 
00214         return app.exec();
00215         }
00216 
00217 #endif
00218 
00219 

Generated on Thu Jul 17 17:23:27 2008 for QVision by  doxygen 1.5.3