main.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, 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/utilite/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(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    dictionary.update();
00205    std::list<int> wordIds2 = dictionary.addNewWords(descriptors2, 2);
00206    UINFO("quantization to words = %d ms", timer.elapsed());
00207 
00208    std::multimap<int, cv::KeyPoint> words1 = aggregate(wordIds1, kpts1);
00209    std::multimap<int, cv::KeyPoint> words2 = aggregate(wordIds2, kpts2);
00210 
00211    // Find pairs
00212    timer.start();
00213    std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
00214    EpipolarGeometry::findPairsUnique(words1, words2, pairs);
00215    UINFO("find pairs = %d ms", timer.elapsed());
00216 
00217    // Find fundamental matrix
00218    timer.start();
00219    std::vector<uchar> status;
00220    cv::Mat fundamentalMatrix = EpipolarGeometry::findFFromWords(pairs, status);
00221    UINFO("inliers = %d/%d", uSum(status), pairs.size());
00222    UINFO("find F = %d ms", timer.elapsed());
00223    if(!fundamentalMatrix.empty())
00224    {
00225            int i = 0;
00226            int goodCount = 0;
00227            for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00228                 {
00229                         if(status[i])
00230                         {
00231                                 // the output of the correspondences can be easily copied in MatLab
00232                                 if(goodCount==0)
00233                                 {
00234                                         printf("x=[%f %f %d]; xp=[%f %f %d];\n",
00235                                                         iter->second.first.pt.x,
00236                                                         iter->second.first.pt.y,
00237                                                         iter->first,
00238                                                         iter->second.second.pt.x,
00239                                                         iter->second.second.pt.y,
00240                                                         iter->first);
00241                                 }
00242                                 else
00243                                 {
00244                                         printf("x=[x;[%f %f %d]]; xp=[xp;[%f %f %d]];\n",
00245                                                         iter->second.first.pt.x,
00246                                                         iter->second.first.pt.y,
00247                                                         iter->first,
00248                                                         iter->second.second.pt.x,
00249                                                         iter->second.second.pt.y,
00250                                                         iter->first);
00251                                 }
00252                                 ++goodCount;
00253                         }
00254                         ++i;
00255                 }
00256 
00257                 // Show the fundamental matrix
00258                 std::cout << "F=" << fundamentalMatrix << std::endl;
00259 
00260                 // Intrinsic parameters K of the camera (guest... non-calibrated camera)
00261                 cv::Mat k = cv::Mat::zeros(3,3,CV_64FC1);
00262                 k.at<double>(0,0) = image1.cols; // focal x
00263                 k.at<double>(1,1) = image1.rows; // focal y
00264                 k.at<double>(2,2) = 1;
00265                 k.at<double>(0,2) = image1.cols/2; // center x in pixels
00266                 k.at<double>(1,2) = image1.rows/2; // center y in pixels
00267 
00268                 // Use essential matrix E=K'*F*K
00269 
00270                 cv::Mat e = k.t()*fundamentalMatrix*k;
00271 
00272                 //remove K from points xe = inv(K)*x
00273                 cv::Mat x1(2, goodCount, CV_64FC1);
00274                 cv::Mat x2(2, goodCount, CV_64FC1);
00275                 i=0;
00276                 int j=0;
00277                 cv::Mat invK = k.inv();
00278                 for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
00279                 {
00280                         if(status[i])
00281                         {
00282                                 cv::Mat tmp(3,1,CV_64FC1);
00283                                 tmp.at<double>(0,0) = iter->second.first.pt.x;
00284                                 tmp.at<double>(1,0) = iter->second.first.pt.y;
00285                                 tmp.at<double>(2,0) = 1;
00286                                 tmp = invK*tmp;
00287                                 x1.at<double>(0,j) = tmp.at<double>(0,0);
00288                                 x1.at<double>(1,j) = tmp.at<double>(1,0);
00289                                 tmp.at<double>(0,0) = iter->second.second.pt.x;
00290                                 tmp.at<double>(1,0) = iter->second.second.pt.y;
00291                                 tmp.at<double>(2,0) = 1;
00292                                 tmp = invK*tmp;
00293                                 x2.at<double>(0,j) = tmp.at<double>(0,0);
00294                                 x2.at<double>(1,j) = tmp.at<double>(1,0);
00295                                 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));
00296                                 ++j;
00297                         }
00298                         ++i;
00299                 }
00300 
00301                 std::cout<<"K=" << k << std::endl;
00302                 timer.start();
00303                 //std::cout<<"e=" << e << std::endl;
00304                 cv::Mat p = EpipolarGeometry::findPFromE(e, x1, x2);
00305                 cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
00306                 p0.at<double>(0,0) = 1;
00307                 p0.at<double>(1,1) = 1;
00308                 p0.at<double>(2,2) = 1;
00309                 UINFO("find P from F = %d ms", timer.elapsed());
00310 
00311                 std::cout<<"P=" << p << std::endl;
00312 
00313                 //find 4D homogeneous points
00314                 cv::Mat x4d;
00315                 timer.start();
00316                 cv::triangulatePoints(p0, p, x1, x2, x4d);
00317                 UINFO("find X (triangulate) = %d ms", timer.elapsed());
00318 
00319                 //Show 4D points
00320                 for(int i=0; i<x4d.cols; ++i)
00321                 {
00322                         x4d.at<double>(0,i) = x4d.at<double>(0,i)/x4d.at<double>(3,i);
00323                         x4d.at<double>(1,i) = x4d.at<double>(1,i)/x4d.at<double>(3,i);
00324                         x4d.at<double>(2,i) = x4d.at<double>(2,i)/x4d.at<double>(3,i);
00325                         x4d.at<double>(3,i) = x4d.at<double>(3,i)/x4d.at<double>(3,i);
00326                         if(i==0)
00327                         {
00328                                 printf("X=[%f;%f;%f;%f];\n",
00329                                         x4d.at<double>(0,i),
00330                                         x4d.at<double>(1,i),
00331                                         x4d.at<double>(2,i),
00332                                         x4d.at<double>(3,i));
00333                         }
00334                         else
00335                         {
00336                                 printf("X=[X [%f;%f;%f;%f]];\n",
00337                                         x4d.at<double>(0,i),
00338                                         x4d.at<double>(1,i),
00339                                         x4d.at<double>(2,i),
00340                                         x4d.at<double>(3,i));
00341                         }
00342                 }
00343 
00344                 //Show rotation/translation of the second camera
00345                 cv::Mat r;
00346                 cv::Mat t;
00347                 EpipolarGeometry::findRTFromP(p, r, t);
00348                 std::cout<< "R=" << r << std::endl;
00349                 std::cout<< "t=" << t << std::endl;
00350 
00351                 //GUI
00352                 QApplication app(argc, argv);
00353                 MainWidget mainWidget(image1, image2, words1, words2, status);
00354                 mainWidget.show();
00355                 app.exec();
00356         }
00357         else
00358         {
00359                 UINFO("Fundamental matrix not found...");
00360         }
00361 
00362     return 0;
00363 }


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:20