main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <opencv2/core/core.hpp>
00029 #include <opencv2/core/types_c.h>
00030 #include <opencv2/highgui/highgui.hpp>
00031 #include <opencv2/imgproc/imgproc.hpp>
00032 #include <iostream>
00033 #include <rtabmap/utilite/ULogger.h>
00034 #include <rtabmap/utilite/UTimer.h>
00035 #include <rtabmap/utilite/UConversion.h>
00036 #include <rtabmap/utilite/UDirectory.h>
00037 #include <rtabmap/utilite/UStl.h>
00038 #include <rtabmap/utilite/UMath.h>
00039 #include <opencv2/calib3d/calib3d.hpp>
00040 #include "rtabmap/core/Features2d.h"
00041 #include "rtabmap/core/EpipolarGeometry.h"
00042 #include "rtabmap/core/VWDictionary.h"
00043 #include "rtabmap/core/Odometry.h"
00044 #include "rtabmap/gui/UCv2Qt.h"
00045 
00046 #include "rtabmap/gui/ImageView.h"
00047 #include "rtabmap/gui/KeypointItem.h"
00048 #include <QApplication>
00049 #include <QGraphicsLineItem>
00050 #include <QGraphicsPixmapItem>
00051 #include <QVBoxLayout>
00052 #include <QHBoxLayout>
00053 #include <QtCore/QTime>
00054 #include <QGraphicsEffect>
00055 
00056 
00057 using namespace rtabmap;
00058 
00059 void showUsage()
00060 {
00061         printf("\nUsage:\n"
00062                         "rtabmap-epipolar_geometry image1.jpg image2.jpg\n");
00063         exit(1);
00064 }
00065 
00066 class MainWidget : public QWidget
00067 {
00068 public:
00069         MainWidget(const cv::Mat & image1,
00070                            const cv::Mat & image2,
00071                            const std::multimap<int, cv::KeyPoint> & words1,
00072                            const std::multimap<int, cv::KeyPoint> & words2,
00073                            const std::vector<uchar> & status)
00074         {
00075                 view1_ = new ImageView(this);
00076                 this->setLayout(new QHBoxLayout());
00077                 this->layout()->setSpacing(0);
00078                 this->layout()->setContentsMargins(0,0,0,0);
00079                 this->layout()->addWidget(view1_);
00080                 view1_->setSceneRect(QRectF(0,0,(float)image1.cols, (float)image1.rows));
00081                 view1_->setLinesShown(true);
00082                 view1_->setFeaturesShown(false);
00083                 view1_->setImageDepthShown(true);
00084 
00085                 view1_->setImage(uCvMat2QImage(image1));
00086                 view1_->setImageDepth(uCvMat2QImage(image2));
00087 
00088                 drawKeypoints(words1, words2, status);
00089         }
00090 protected:
00091         virtual void showEvent(QShowEvent* event)
00092         {
00093                 resizeEvent(0);
00094         }
00095 private:
00096         void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords, const std::vector<uchar> & status)
00097         {
00098                 UTimer timer;
00099 
00100                 timer.start();
00101                 QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
00102                 QList<bool> inliers;
00103                 int j=0;
00104                 for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
00105                 {
00106                         int id = (*i).first;
00107                         QColor color;
00108                         if(uContains(loopWords, id))
00109                         {
00110                                 // PINK = FOUND IN LOOP SIGNATURE
00111                                 color = Qt::magenta;
00112                                 //To draw lines... get only unique correspondences
00113                                 if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
00114                                 {
00115                                         uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(i->second.pt, uValues(loopWords, id).begin()->pt));
00116                                         inliers.push_back(status[j++]);
00117                                 }
00118                         }
00119                         else if(refWords.count(id) > 1)
00120                         {
00121                                 // YELLOW = NEW and multiple times
00122                                 color = Qt::yellow;
00123                         }
00124                         else
00125                         {
00126                                 // GREEN = NEW
00127                                 color = Qt::green;
00128                         }
00129                         view1_->addFeature(id, i->second, 0, color);
00130                 }
00131                 ULOGGER_DEBUG("source time = %f s", timer.ticks());
00132 
00133                 // Draw lines between corresponding features...
00134                 UASSERT(uniqueCorrespondences.size() == inliers.size());
00135                 QList<bool>::iterator jter = inliers.begin();
00136                 for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
00137                         iter!=uniqueCorrespondences.end();
00138                         ++iter)
00139                 {
00140                         view1_->addLine(
00141                                         iter->first.x,
00142                                         iter->first.y,
00143                                         iter->second.x,
00144                                         iter->second.y,
00145                                         *jter?Qt::cyan:Qt::red);
00146                         ++jter;
00147                 }
00148                 view1_->update();
00149         }
00150 
00151 private:
00152         ImageView * view1_;
00153 };
00154 
00155 std::multimap<int, cv::KeyPoint> aggregate(const std::list<int> & wordIds, const std::vector<cv::KeyPoint> & keypoints)
00156 {
00157         std::multimap<int, cv::KeyPoint> words;
00158         std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
00159         for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
00160         {
00161                 words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
00162                 ++kpIter;
00163         }
00164         return words;
00165 }
00166 
00167 
00168 
00169 int main(int argc, char** argv)
00170 {
00171         ULogger::setType(ULogger::kTypeConsole);
00172         ULogger::setLevel(ULogger::kInfo);
00173 
00174         cv::Mat image1;
00175     cv::Mat image2;
00176         if(argc == 3)
00177         {
00178                 image1 = cv::imread(argv[1]);
00179                 image2 = cv::imread(argv[2]);
00180         }
00181         else
00182         {
00183                 showUsage();
00184         }
00185 
00186         QTime timer;
00187         timer.start();
00188 
00189         // Extract words
00190         timer.start();
00191    VWDictionary dictionary;
00192    ParametersMap param;
00193    param.insert(ParametersPair(Parameters::kSURFExtended(), "true"));
00194    param.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "100"));
00195    SURF detector(param);
00196    std::vector<cv::KeyPoint> kpts1 = detector.generateKeypoints(image1);
00197    std::vector<cv::KeyPoint> kpts2 = detector.generateKeypoints(image2);
00198    cv::Mat descriptors1 = detector.generateDescriptors(image1, kpts1);
00199    cv::Mat descriptors2 = detector.generateDescriptors(image2, kpts2);
00200    UINFO("detect/extract features = %d ms", timer.elapsed());
00201 
00202    timer.start();
00203    std::list<int> wordIds1 = dictionary.addNewWords(descriptors1, 1);
00204    std::list<int> wordIds2 = dictionary.addNewWords(descriptors2, 2);
00205    UINFO("quantization to words = %d ms", timer.elapsed());
00206 
00207    std::multimap<int, cv::KeyPoint> words1 = aggregate(wordIds1, kpts1);
00208    std::multimap<int, cv::KeyPoint> words2 = aggregate(wordIds2, kpts2);
00209 
00210    // Find pairs
00211    timer.start();
00212    std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
00213    EpipolarGeometry::findPairsUnique(words1, words2, pairs);
00214    UINFO("find pairs = %d ms", timer.elapsed());
00215 
00216    // Find fundamental matrix
00217    timer.start();
00218    std::vector<uchar> status;
00219    cv::Mat fundamentalMatrix = EpipolarGeometry::findFFromWords(pairs, status);
00220    UINFO("inliers = %d/%d", uSum(status), pairs.size());
00221    UINFO("find F = %d ms", timer.elapsed());
00222    if(!fundamentalMatrix.empty())
00223    {
00224            int i = 0;
00225            int goodCount = 0;
00226            for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00227                 {
00228                         if(status[i])
00229                         {
00230                                 // the output of the correspondences can be easily copied in MatLab
00231                                 if(goodCount==0)
00232                                 {
00233                                         printf("x=[%f %f %d]; xp=[%f %f %d];\n",
00234                                                         iter->second.first.pt.x,
00235                                                         iter->second.first.pt.y,
00236                                                         iter->first,
00237                                                         iter->second.second.pt.x,
00238                                                         iter->second.second.pt.y,
00239                                                         iter->first);
00240                                 }
00241                                 else
00242                                 {
00243                                         printf("x=[x;[%f %f %d]]; xp=[xp;[%f %f %d]];\n",
00244                                                         iter->second.first.pt.x,
00245                                                         iter->second.first.pt.y,
00246                                                         iter->first,
00247                                                         iter->second.second.pt.x,
00248                                                         iter->second.second.pt.y,
00249                                                         iter->first);
00250                                 }
00251                                 ++goodCount;
00252                         }
00253                         ++i;
00254                 }
00255 
00256                 // Show the fundamental matrix
00257                 std::cout << "F=" << fundamentalMatrix << std::endl;
00258 
00259                 // Intrinsic parameters K of the camera (guest... non-calibrated camera)
00260                 cv::Mat k = cv::Mat::zeros(3,3,CV_64FC1);
00261                 k.at<double>(0,0) = image1.cols; // focal x
00262                 k.at<double>(1,1) = image1.rows; // focal y
00263                 k.at<double>(2,2) = 1;
00264                 k.at<double>(0,2) = image1.cols/2; // center x in pixels
00265                 k.at<double>(1,2) = image1.rows/2; // center y in pixels
00266 
00267                 // Use essential matrix E=K'*F*K
00268 
00269                 cv::Mat e = k.t()*fundamentalMatrix*k;
00270 
00271                 //remove K from points xe = inv(K)*x
00272                 cv::Mat x1(2, goodCount, CV_64FC1);
00273                 cv::Mat x2(2, goodCount, CV_64FC1);
00274                 i=0;
00275                 int j=0;
00276                 cv::Mat invK = k.inv();
00277                 for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00278                 {
00279                         if(status[i])
00280                         {
00281                                 cv::Mat tmp(3,1,CV_64FC1);
00282                                 tmp.at<double>(0,0) = iter->second.first.pt.x;
00283                                 tmp.at<double>(1,0) = iter->second.first.pt.y;
00284                                 tmp.at<double>(2,0) = 1;
00285                                 tmp = invK*tmp;
00286                                 x1.at<double>(0,j) = tmp.at<double>(0,0);
00287                                 x1.at<double>(1,j) = tmp.at<double>(1,0);
00288                                 tmp.at<double>(0,0) = iter->second.second.pt.x;
00289                                 tmp.at<double>(1,0) = iter->second.second.pt.y;
00290                                 tmp.at<double>(2,0) = 1;
00291                                 tmp = invK*tmp;
00292                                 x2.at<double>(0,j) = tmp.at<double>(0,0);
00293                                 x2.at<double>(1,j) = tmp.at<double>(1,0);
00294                                 UDEBUG("i=%d j=%d, x1=[%f,%f] x2=[%f,%f]", i, j, x1.at<double>(0,j), x1.at<double>(1,j), x2.at<double>(0,j), x2.at<double>(1,j));
00295                                 ++j;
00296                         }
00297                         ++i;
00298                 }
00299 
00300                 std::cout<<"K=" << k << std::endl;
00301                 timer.start();
00302                 //std::cout<<"e=" << e << std::endl;
00303                 cv::Mat p = EpipolarGeometry::findPFromE(e, x1, x2);
00304                 cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
00305                 p0.at<double>(0,0) = 1;
00306                 p0.at<double>(1,1) = 1;
00307                 p0.at<double>(2,2) = 1;
00308                 UINFO("find P from F = %d ms", timer.elapsed());
00309 
00310                 std::cout<<"P=" << p << std::endl;
00311 
00312                 //find 4D homogeneous points
00313                 cv::Mat x4d;
00314                 timer.start();
00315                 cv::triangulatePoints(p0, p, x1, x2, x4d);
00316                 UINFO("find X (triangulate) = %d ms", timer.elapsed());
00317 
00318                 //Show 4D points
00319                 for(int i=0; i<x4d.cols; ++i)
00320                 {
00321                         x4d.at<double>(0,i) = x4d.at<double>(0,i)/x4d.at<double>(3,i);
00322                         x4d.at<double>(1,i) = x4d.at<double>(1,i)/x4d.at<double>(3,i);
00323                         x4d.at<double>(2,i) = x4d.at<double>(2,i)/x4d.at<double>(3,i);
00324                         x4d.at<double>(3,i) = x4d.at<double>(3,i)/x4d.at<double>(3,i);
00325                         if(i==0)
00326                         {
00327                                 printf("X=[%f;%f;%f;%f];\n",
00328                                         x4d.at<double>(0,i),
00329                                         x4d.at<double>(1,i),
00330                                         x4d.at<double>(2,i),
00331                                         x4d.at<double>(3,i));
00332                         }
00333                         else
00334                         {
00335                                 printf("X=[X [%f;%f;%f;%f]];\n",
00336                                         x4d.at<double>(0,i),
00337                                         x4d.at<double>(1,i),
00338                                         x4d.at<double>(2,i),
00339                                         x4d.at<double>(3,i));
00340                         }
00341                 }
00342 
00343                 //Show rotation/translation of the second camera
00344                 cv::Mat r;
00345                 cv::Mat t;
00346                 EpipolarGeometry::findRTFromP(p, r, t);
00347                 std::cout<< "R=" << r << std::endl;
00348                 std::cout<< "t=" << t << std::endl;
00349 
00350                 //GUI
00351                 QApplication app(argc, argv);
00352                 MainWidget mainWidget(image1, image2, words1, words2, status);
00353                 mainWidget.show();
00354                 app.exec();
00355         }
00356         else
00357         {
00358                 UINFO("Fundamental matrix not found...");
00359         }
00360 
00361     return 0;
00362 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Fri Aug 28 2015 12:51:31