00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00111 color = Qt::magenta;
00112
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
00122 color = Qt::yellow;
00123 }
00124 else
00125 {
00126
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
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
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
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
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
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
00258 std::cout << "F=" << fundamentalMatrix << std::endl;
00259
00260
00261 cv::Mat k = cv::Mat::zeros(3,3,CV_64FC1);
00262 k.at<double>(0,0) = image1.cols;
00263 k.at<double>(1,1) = image1.rows;
00264 k.at<double>(2,2) = 1;
00265 k.at<double>(0,2) = image1.cols/2;
00266 k.at<double>(1,2) = image1.rows/2;
00267
00268
00269
00270 cv::Mat e = k.t()*fundamentalMatrix*k;
00271
00272
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
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
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
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
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
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 }