QVision: Qt's Image, Video and Computer Vision Library

00001 /*
00002  *      Copyright (C) 2012. 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 
00039 // Uncomment the line below to show the initial and the optimized reconstruction.
00040 #define SHOW_RECONSTRUCTION
00041 
00042 #include <QVApplication>
00043 #include <QTime>
00044 #include <qvsfm.h>
00045 #include <sfmviewer.h>
00046 #include <qvros.h>
00047 
00048 // Print the number of cameras, 3D points, projections and some statistics from a SfM reconstruction.
00049 void printReconstructionStats(  const QList<QVCameraPose> &cameraPoses,
00050                                 const QList<QV3DPointF> &points3D,
00051                                 const QList< QHash< int, QPointF> > &pointsProjections)
00052         {
00053         const int numCameras = cameraPoses.count(), numPoints = points3D.count();
00054         int numProjections = 0;
00055         QHash< int, QPointF> temp;
00056         foreach(temp, pointsProjections)
00057                 numProjections += temp.count();
00058 
00059         std::cout << "[main] Readed "   << numPoints << " points, "
00060                                         << numCameras << " cameras, "
00061                                         << numProjections << " projections, "
00062                                         << double(numProjections)/double(numCameras) << " projections/camera, "
00063                                         << double(numProjections)/double(numPoints) << " projections/point."
00064                                         << std::endl;
00065         }
00066 
00067 int main(int argc, char *argv[])
00068         {
00069         #ifdef SHOW_RECONSTRUCTION
00070         QVApplication app(argc, argv, "Example application for QVision.", true);
00071         #else
00072         QVApplication app(argc, argv, "Example application for QVision.", false);
00073         #endif
00074 
00075         // 1. Get the data-set files path from the first command line parameter.
00076         if (app.getNumberOfArguments() < 2)
00077                 {
00078                 std::cout << "Usage: " << std::endl << "\t" << argv[0] << " <data_set_path> [iterations] [lambda]"
00079                         << std::endl << std::endl;
00080                 exit(0);
00081                 }
00082 
00083         const QString path = app.getArgument(1);
00084 
00085         // 2. Set the number of iterations, and lambda values for the GEA and sSBA optimizations, if provided as parameters through the command line.
00086         bool iters_OK = true, lambda_OK = true;
00087 
00088         const int iters =                       (app.getNumberOfArguments() < 3)? 10 : app.getArgument(2).toInt(&iters_OK);
00089         const double lambda =           (app.getNumberOfArguments() < 4)? 1e-3 : app.getArgument(3).toDouble(&lambda_OK);
00090 
00091         if (not iters_OK)
00092                 {
00093                 std::cout << "[main] Error reading number of iterations." << std::endl;
00094                 exit(0);
00095                 }
00096         else
00097                 std::cout << "[main] Number of iterations = " << iters << std::endl;
00098 
00099         if (not lambda_OK)
00100                 {
00101                 std::cout << "[main] Error reading lambda parameter" << std::endl;
00102                 exit(0);
00103                 }
00104         else
00105                 std::cout << "[main] Lambda = " << lambda << std::endl;
00106 
00107         // 3. Read SfM reconstruction from the specified path.
00108         QList<QV3DPointF> points3D;
00109         QList<QVCameraPose> cameraPoses;
00110         QList<QVMatrix> cameraCalibrations;
00111         QList< QHash< int, QPointF> > pointsProjections;
00112 
00113         if( not readSfMReconstruction(path, cameraCalibrations, cameraPoses, points3D, pointsProjections) )
00114                 {
00115                 std::cout << "[main] Error: could not read SfM reconstruction from path '" << qPrintable(path) << "'." << std::endl;
00116                 return 0;
00117                 }
00118 
00119         // 4. Correct camera intrinsic calibrations from point projections.
00120         const QList< QHash< int, QPointF> > calibratedPointsProjections = correctIntrinsics(cameraCalibrations, pointsProjections);
00121 
00122         // 5. Correct camera cheirality for the 3D structure.
00123         if (testCheirality(cameraPoses, calibratedPointsProjections))
00124                 invertCheirality(cameraPoses, points3D);
00125 
00126         // 6. Print SfM reconstruction stats.
00127         std::cout << "[main] Reconstruction loaded."<< std::endl;       
00128         printReconstructionStats(cameraPoses, points3D, calibratedPointsProjections);
00129 
00130         QList<QV3DPointF> points3D_SBA;
00131         QList<QVCameraPose> cameraPoses_SBA;
00132         
00133         int time_sSBA;
00134         sSBAOptimization(cameraPoses, points3D, calibratedPointsProjections, cameraPoses_SBA, points3D_SBA, time_sSBA, iters, lambda, true);
00135 
00136         // 9. Print errors and total performance times.
00137         std::cout << "[main] Initial error\t" << 1000.0 * reconstructionError(cameraPoses, points3D, calibratedPointsProjections) << std::endl;
00138         std::cout << "[main] sSBA error\t" << 1000.0 * reconstructionError(cameraPoses_SBA, points3D_SBA, calibratedPointsProjections) << std::endl;
00139 
00140         #ifdef SHOW_RECONSTRUCTION
00141         SfMViewer originalMap(cameraPoses, points3D, "Original reconstruction");
00142         SfMViewer SBAMap(cameraPoses_SBA, points3D_SBA, "Reconstruction optimized with SBA");
00143         return app.exec();
00144         #endif // SHOW_RECONSTRUCTION
00145 
00146         exit(0);
00147         }
00148
examples/testSBA_ROS/testSBA_ROS.cpp
