examples/rectify/rectify.cpp

Go to the documentation of this file.
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 <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>      // Basic TooN library
00056 #include <TooN/numhelpers.h>    // If you want a few extras
00057 #include <TooN/SVD.h>           // If you want singular value decompositions
00058 #include <TooN/LU.h>            // If you want lu triangular decompositions
00059 #include <TooN/SymEigen.h>      // If you want Eigen decomposition of symmetric matrices
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 // x < 0
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 // This is suposed to sort the points in a list, by... who knows?
00145 // Anyway, it doesn't works
00146 bool SortTemplatePoints(QList<QPoint> &points)
00147         {
00148         if (points.size() != 5)
00149                 return false;
00150 
00151         QList<QPoint> result;
00152 
00153         // This array will keep the index of the points.
00154         uInt index[5];
00155 
00156         // Calculate the index of the central point
00157         uInt indexp = getClosestPointIndex(getMeanPoint(points), points);
00158 
00159         result.append(points.at(indexp));
00160         points.removeAt(indexp);
00161 
00162         // Calculate the index of the top left point
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         // Calculate the other points
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 // This function returns the homography that maps the points from a source position to a
00200 // destination position
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         // First, state the coefs matrix for the homogeneous linear system of equations, of the form:
00210         //      Ax = 0
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         // create the SVD of M
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 // This function retunrs an integer value that indicates how well the homography maps the source points
00259 // in a list, to the points in the destination list.
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 // This function creates a list of the template points, first with the center point,
00284 // then the rest of the points following a clockwise order around the center point.
00285 Matrix<> GetTemplateMatrixPoints()
00286         {
00287         //                                      Center          Norwest         Noreast         Southeast       Southwest
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 // This function sustitudes warpPerspective from the qvipp, and applies the planar transformation H
00292 // to a source image, storing the transformed image in the destination image
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 // This function transforms a list of n points, to a nx3 matrix, containing the rows the
00316 // homogeneous coordinates for each point in the list.
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                         // Harris corner response image
00361                         QVImage<sFloat> temp(cols, rows), cornerResponseImage(cols, rows);
00362                         SobelCornerResponseImage(image, cornerResponseImage);
00363 
00364                         timeFlag("Corner response image");
00365 
00367                         // Hot points
00368                         QList<QPoint> hotPoints;
00369                         GetHotPoints(cornerResponseImage, hotPoints, sizeMax);
00370 
00371                         timeFlag("Get hotpoints");
00372 
00374                         // Calibración
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 

Generated on Thu Mar 13 19:18:16 2008 for QVision by  doxygen 1.5.3