00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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 <QVDefaultGUI>
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.h>
00061 #include <qvip.h>
00062 #include <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
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
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
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
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
00205 QVDefaultGUI 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