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 <math.h>
00042
00043 #include <QDebug>
00044 #include <QVector>
00045 #include <QThread>
00046
00047 #include <qvcore/qvapplication.h>
00048 #include <qvcameras/qvmplayercamera.h>
00049 #include <qvgui/qvgui.h>
00050
00051 #include <qvdta/qvpolyline.h>
00052 #include <qvdta/qvdta.h>
00053
00054 #include <TooN/TooN.h>
00055 #include <TooN/numerics.h>
00056 #include <TooN/numhelpers.h>
00057 #include <TooN/SVD.h>
00058 #include <TooN/LU.h>
00059 #include <TooN/SymEigen.h>
00060
00061 #define PI 3.1415926535
00062
00063 Q_DECLARE_METATYPE(Matrix<>);
00064
00066
00067 void GetHotPoints(const QVImage<sFloat> cornerResponseImage, QList<QPoint> &hotPoints, uInt maxWindow)
00068 {
00069 const uInt rows = cornerResponseImage.getRows(), cols = cornerResponseImage.getCols();
00070
00071 QVImage<uChar> binaryCornerImage(cols, rows);
00072 FilterLocalMax(cornerResponseImage, binaryCornerImage, maxWindow, maxWindow);
00073
00074 QVIMAGE_INIT_READ(uChar,binaryCornerImage);
00075 for (uInt row = 0; row < binaryCornerImage.getRows(); row++)
00076 for (uInt col = 0; col < binaryCornerImage.getCols(); col++)
00077 if (QVIMAGE_PIXEL(binaryCornerImage, col, row,0))
00078 hotPoints.append(QPoint(col, row));
00079 }
00080
00081 void GetMaximalPoints(const QVImage<sFloat> cornerResponseImage, QList<QPoint> &hotPoints, QList<QPoint> &maximalPoints, uInt maxPoints)
00082 {
00083 while( hotPoints.size() > 0 && maximalPoints.size() < maxPoints )
00084 {
00085 uInt maxIndex = 0;
00086 for (int n=0; n < hotPoints.size(); n++)
00087 if ( cornerResponseImage(hotPoints.at(n)) > cornerResponseImage(hotPoints.at(maxIndex)) )
00088 maxIndex = n;
00089
00090 maximalPoints.append(hotPoints.at(maxIndex));
00091 hotPoints.removeAt(maxIndex);
00092 }
00093 }
00094
00095 uInt getClosestPointIndex(const QPoint point, const QList<QPoint> &pointList)
00096 {
00097 uInt index = 0;
00098 for (uInt n = 1; n < pointList.size(); n++)
00099 if ((point - pointList.at(n)).manhattanLength() < (point - pointList.at(index)).manhattanLength())
00100 index = n;
00101
00102 return index;
00103 }
00104
00105 QPoint getMeanPoint(const QList<QPoint> &pointList)
00106 {
00107 QPoint center(0,0);
00108
00109 for (uInt n = 0; n < pointList.size(); n++)
00110 {
00111 center.rx() += pointList.at(n).x();
00112 center.ry() += pointList.at(n).y();
00113 }
00114
00115 center.rx() = center.rx() / pointList.size();
00116 center.ry() = center.ry() / pointList.size();
00117
00118 return center;
00119 }
00120
00121 double angle(const QPoint &p)
00122 {
00123 double x = p.x(), y = p.y();
00124 if (x>0)
00125 if (y>=0)
00126 return atan(y/x);
00127 else
00128 return atan(y/x) + 2*PI;
00129 else if (x == 0)
00130 if (y>0)
00131 return PI/2;
00132 else
00133 return 3*PI/2;
00134 else
00135 return atan(y/x)+PI;
00136 }
00137
00138 double clockWiseAngle(const QPoint &p1, const QPoint &p2)
00139 {
00140 double clockAngle = angle(p2) - angle(p1);
00141 return (clockAngle < 0)? clockAngle + 2*PI:clockAngle;
00142 }
00143
00144
00145
00146 bool SortTemplatePoints(QList<QPoint> &points)
00147 {
00148 if (points.size() != 5)
00149 return false;
00150
00151 QList<QPoint> result;
00152
00153
00154 uInt index[5];
00155
00156
00157 uInt indexp = getClosestPointIndex(getMeanPoint(points), points);
00158
00159 result.append(points.at(indexp));
00160 points.removeAt(indexp);
00161
00162
00163 double minDistance = 1000000;
00164 for (uInt n = 0; n < points.size(); n++)
00165 if ( points.at(n).manhattanLength() < minDistance )
00166 {
00167 minDistance = points.at(n).manhattanLength();
00168 indexp = n;
00169 }
00170
00171 result.append(points.at(indexp));
00172 points.removeAt(indexp);
00173
00174
00175 while(points.size() > 0)
00176 {
00177 indexp = 0;
00178 double minAngle = clockWiseAngle( result.back() - result.front(), points.at(indexp) - result.front());
00179
00180 for (uInt n = 1; n < points.size(); n++)
00181 {
00182 double actualAngle = clockWiseAngle( result.back() - result.front(), points.at(n) - result.front());
00183 if ( actualAngle < minAngle )
00184 {
00185 minAngle = actualAngle;
00186 indexp = n;
00187 }
00188 }
00189
00190 result.append(points.at(indexp));
00191 points.removeAt(indexp);
00192 }
00193
00194 points = result;
00195
00196 return true;
00197 }
00198
00199
00200
00201 Matrix<> CalibrateHomography(const Matrix<> &sourcePointsMatrix, const Matrix<> &destinationPoints)
00202 {
00203 Q_ASSERT(sourcePointsMatrix.num_cols() == 2);
00204 Q_ASSERT(sourcePointsMatrix.num_cols() == destinationPoints.num_cols());
00205 Q_ASSERT(sourcePointsMatrix.num_rows() == destinationPoints.num_rows());
00206
00207 const uInt rows = sourcePointsMatrix.num_rows();
00208
00209
00210
00211 Matrix<> coefsMatrix(3*rows,9);
00212
00213 for (uInt n = 0; n < rows; n++)
00214 {
00215 double x = sourcePointsMatrix[n][0], y = sourcePointsMatrix[n][1],
00216 p = destinationPoints[n][0], q = destinationPoints[n][1];
00217
00218 double equation1[9] = { 0, 0, 0, -x, -y, -1, q*x, q*y, q},
00219 equation2[9] = { x, y, 1, 0, 0, 0, -p*x, -p*y, -p},
00220 equation3[9] = { -q*x, -q*y, -q, p*x, p*y, p, 0, 0, 0};
00221
00222 coefsMatrix[3*n] = Vector<9>(equation1);
00223 coefsMatrix[3*n+1] = Vector<9>(equation2);
00224 coefsMatrix[3*n+2] = Vector<9>(equation3);
00225 }
00226
00227
00228 SVD<> svdCoefsMatrix(coefsMatrix);
00229
00230 Vector<9> x = svdCoefsMatrix.get_VT()[8];
00231 Matrix<> homography(3,3);
00232
00233 homography[0][0] = x[0]; homography[0][1] = x[1]; homography[0][2] = x[2];
00234 homography[1][0] = x[3]; homography[1][1] = x[4]; homography[1][2] = x[5];
00235 homography[2][0] = x[6]; homography[2][1] = x[7]; homography[2][2] = x[8];
00236
00237 return homography;
00238 }
00239
00240 void normalizeHomogeneousCoordinates(Matrix<> &points)
00241 {
00242 const uInt cols = points.num_cols(), rows = points.num_rows();
00243
00244 for (uInt i = 0; i < rows; i++)
00245 for (uInt j = 0; j < cols; j++)
00246 points[i][j] /= points[i][cols-1];
00247 }
00248
00249 void normalizeHomogeneousCoordinates(Matrix<1,3> &points)
00250 {
00251 const uInt cols = points.num_cols(), rows = points.num_rows();
00252
00253 for (uInt i = 0; i < rows; i++)
00254 for (uInt j = 0; j < cols; j++)
00255 points[i][j] /= points[i][cols-1];
00256 }
00257
00258
00259
00260 double testErrorHomography(const Matrix<> &sourcePointsMatrix, const Matrix<> &destinationPoints, const Matrix<> homography)
00261 {
00262 const uInt cols = sourcePointsMatrix.num_cols(), rows = sourcePointsMatrix.num_rows();
00263
00264 Matrix <> projectedPoints(sourcePointsMatrix.num_rows(),3);
00265 Matrix <> residuals (sourcePointsMatrix.num_rows(),3);
00266
00267 projectedPoints = sourcePointsMatrix * homography.T();
00268 normalizeHomogeneousCoordinates(projectedPoints);
00269 residuals = projectedPoints - destinationPoints;
00270
00271 double accum = 0;
00272 for (uInt i = 0; i < rows; i++)
00273 {
00274 double square = 0;
00275 for (uInt j = 0; j < cols; j++)
00276 square += residuals[i][j]*residuals[i][j];
00277 accum += sqrt(square);
00278 }
00279
00280 return accum;
00281 }
00282
00283
00284
00285 Matrix<> GetTemplateMatrixPoints()
00286 {
00287
00288 return Matrix<5,3> ((double[5][3]){ 0,0,1, -1,+1,+1, +1,+1,+1, +1,-1,+1, -1,-1,+1});
00289 }
00290
00291
00292
00293 void myWarpPerspective(const QVImage<uChar> &src, QVImage<uChar> &dest, const Matrix <> H, const double zoom)
00294 {
00295 const double cols = src.getCols(), rows = src.getRows();
00296 const Matrix<> Hinv = SVD<>(H).get_pinv();
00297
00298 for (double col = 0; col < cols; col++)
00299 for (double row = 0; row < rows; row++)
00300 {
00301 const double x = col, y = row;
00302
00303 double v[3] = { 2*(x - cols/2)/cols, -2*(y - rows/2)/cols, 1 };
00304
00305 Vector <3> vP;
00306 vP = H * Vector<3>(v);
00307 const double x0 = vP[0]/vP[2], y0 = vP[1]/vP[2];
00308 const QPoint p2 = QPoint( (uInt) x, (uInt) y ), p1 = QPoint( zoom*x0 + cols/2, -zoom*y0 + rows/2 );
00309
00310 if (dest.getROI().contains(p2) && src.getROI().contains(p1))
00311 dest(p1) = src(p2);
00312 }
00313 }
00314
00315
00316
00317 void pointListToMatrix(const QVImage<uChar> &image, const QList<QPoint> &points, Matrix<> &matrix)
00318 {
00319 Q_ASSERT(points.size() == matrix.get_rows());
00320 Q_ASSERT(3 == matrix.get_cols());
00321 const double rows = image.getRows(), cols = image.getCols();
00322
00323 for (uInt n = 0; n < points.size(); n++)
00324 {
00325 const double p = points.at(n).x(), q = points.at(n).y();
00326 double v[3] = { 2*(p - cols/2)/cols, -2*(q - rows/2)/cols, 1 };
00327 matrix[n] = Vector<3>(v);
00328 }
00329 }
00330
00331 class MyWorker: public QVWorker
00332 {
00333 public:
00334 MyWorker(QString name): QVWorker(name)
00335 {
00336 addProperty<double>("Max error", inputFlag, 0.02, "for an homography to be considered good", 0, 0.1);
00337 addProperty<double>("Zoom", inputFlag, 50, "Size of the rectified template", 1, 100);
00338 addProperty<double>("Focal", outputFlag, 0, "Focal distance");
00339 addProperty<int>("Window size", inputFlag, 10, "Corner response window search", 1, 100);
00340 addProperty< QVImage<uChar,1> >("Input image", inputFlag|outputFlag);
00341 addProperty< QVImage<uChar,3> >("Corners", outputFlag);
00342 addProperty< QVImage<uChar,1> >("Wrapped", outputFlag);
00343 }
00344
00345 void iterate()
00346 {
00347 const QVImage<uChar> image = getPropertyValue< QVImage<uChar,1> >("Input image");
00348 const uInt rows = image.getRows(), cols = image.getCols(),
00349 sizeMax = getPropertyValue<int>("Window size");
00350
00351 const double maxError = getPropertyValue<double>("Max error"),
00352 zoom = getPropertyValue<double>("Zoom"),
00353 focal = getPropertyValue<double>("Focal");
00354
00355 QVImage<uChar,3> destino = image;
00356
00357 timeFlag("grab Frame");
00358
00360
00361 QVImage<sFloat> temp(cols, rows), cornerResponseImage(cols, rows);
00362 SobelCornerResponseImage(image, cornerResponseImage);
00363
00364 timeFlag("Corner response image");
00365
00367
00368 QList<QPoint> hotPoints;
00369 GetHotPoints(cornerResponseImage, hotPoints, sizeMax);
00370
00371 timeFlag("Get hotpoints");
00372
00374
00375 QList<QPoint> maximalPoints;
00376 GetMaximalPoints(cornerResponseImage, hotPoints, maximalPoints, 5);
00377 SortTemplatePoints(maximalPoints);
00378
00379 drawPoints(maximalPoints, destino);
00380
00381 timeFlag("Get max hotpoints");
00382
00383 if (maximalPoints.size() == 5)
00384 {
00385 Matrix <> sourcePointsMatrix(5,3), destinationPointsMatrix(5,3);
00386 destinationPointsMatrix = GetTemplateMatrixPoints();
00387 pointListToMatrix(image, maximalPoints, sourcePointsMatrix);
00388
00389 Matrix<> H = CalibrateHomography(sourcePointsMatrix, destinationPointsMatrix);
00390
00391 const double actualError = testErrorHomography(sourcePointsMatrix, destinationPointsMatrix, H);
00392 if (actualError < maxError)
00393 {
00394 QVImage<uChar> wrapped(cols, rows);
00395 Set(wrapped,0);
00396
00397 myWarpPerspective(image, wrapped, H, zoom);
00398
00399 setPropertyValue< QVImage<uChar,1> >("Wrapped", wrapped);
00400 }
00401 }
00402
00403 timeFlag("Calibrate");
00404
00405 setPropertyValue< QVImage<uChar,3> >("Corners", destino);
00406 timeFlag("Draw corners");
00407 }
00408 };
00409
00410 int main(int argc, char *argv[])
00411 {
00412 QVApplication app(argc, argv,
00413
00414 "Example program for QVision library. Applies corner detection over an input video."
00415
00416 );
00417
00418 QVMPlayerCamera camera("Video");
00419 MyWorker worker("Corners Worker");
00420 camera.link(&worker,"Input image");
00421
00422 QVGUI interface;
00423
00424 QVImageCanvas imageCanvas("Corners");
00425 imageCanvas.linkProperty(worker, "Corners");
00426
00427 QVImageCanvas imageCanvas2("Wrapped");
00428 imageCanvas2.linkProperty(worker, "Wrapped");
00429
00430 return app.exec();
00431 }
00432
00434