tools/EpipolarGeometry/main.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <opencv2/core/core.hpp>
29 #include <opencv2/core/types_c.h>
30 #include <opencv2/highgui/highgui.hpp>
31 #include <opencv2/imgproc/imgproc.hpp>
32 #include <iostream>
33 #include <rtabmap/utilite/ULogger.h>
34 #include <rtabmap/utilite/UTimer.h>
35 #include <rtabmap/utilite/UConversion.h>
36 #include <rtabmap/utilite/UDirectory.h>
37 #include <rtabmap/utilite/UStl.h>
38 #include <rtabmap/utilite/UMath.h>
39 #include <opencv2/calib3d/calib3d.hpp>
40 #include "rtabmap/core/Features2d.h"
41 #include "rtabmap/core/EpipolarGeometry.h"
42 #include "rtabmap/core/VWDictionary.h"
43 #include "rtabmap/core/Odometry.h"
44 #include "rtabmap/utilite/UCv2Qt.h"
45 
46 #include "rtabmap/gui/ImageView.h"
47 #include "rtabmap/gui/KeypointItem.h"
48 #include <QApplication>
49 #include <QGraphicsLineItem>
50 #include <QGraphicsPixmapItem>
51 #include <QVBoxLayout>
52 #include <QHBoxLayout>
53 #include <QtCore/QTime>
54 #include <QGraphicsEffect>
55 
56 
57 using namespace rtabmap;
58 
59 void showUsage()
60 {
61  printf("\nUsage:\n"
62  "rtabmap-epipolar_geometry image1.jpg image2.jpg\n");
63  exit(1);
64 }
65 
66 class MainWidget : public QWidget
67 {
68 public:
69  MainWidget(const cv::Mat & image1,
70  const cv::Mat & image2,
71  const std::multimap<int, cv::KeyPoint> & words1,
72  const std::multimap<int, cv::KeyPoint> & words2,
73  const std::vector<uchar> & status)
74  {
75  view1_ = new ImageView(this);
76  this->setLayout(new QHBoxLayout());
77  this->layout()->setSpacing(0);
78  this->layout()->setContentsMargins(0,0,0,0);
79  this->layout()->addWidget(view1_);
80  view1_->setSceneRect(QRectF(0,0,(float)image1.cols, (float)image1.rows));
81  view1_->setLinesShown(true);
82  view1_->setFeaturesShown(false);
83  view1_->setImageDepthShown(true);
84 
85  view1_->setImage(uCvMat2QImage(image1));
86  view1_->setImageDepth(image2);
87 
88  drawKeypoints(words1, words2, status);
89  }
90 protected:
91  virtual void showEvent(QShowEvent* event)
92  {
93  resizeEvent(0);
94  }
95 private:
96  void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords, const std::vector<uchar> & status)
97  {
98  UTimer timer;
99 
100  timer.start();
101  QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
102  QList<bool> inliers;
103  int j=0;
104  for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
105  {
106  int id = (*i).first;
107  QColor color;
108  if(uContains(loopWords, id))
109  {
110  // PINK = FOUND IN LOOP SIGNATURE
111  color = Qt::magenta;
112  //To draw lines... get only unique correspondences
113  if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
114  {
115  uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(i->second.pt, uValues(loopWords, id).begin()->pt));
116  inliers.push_back(status[j++]);
117  }
118  }
119  else if(refWords.count(id) > 1)
120  {
121  // YELLOW = NEW and multiple times
122  color = Qt::yellow;
123  }
124  else
125  {
126  // GREEN = NEW
127  color = Qt::green;
128  }
129  view1_->addFeature(id, i->second, 0, color);
130  }
131  ULOGGER_DEBUG("source time = %f s", timer.ticks());
132 
133  // Draw lines between corresponding features...
134  UASSERT(uniqueCorrespondences.size() == inliers.size());
135  QList<bool>::iterator jter = inliers.begin();
136  for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
137  iter!=uniqueCorrespondences.end();
138  ++iter)
139  {
140  view1_->addLine(
141  iter->first.x,
142  iter->first.y,
143  iter->second.x,
144  iter->second.y,
145  *jter?Qt::cyan:Qt::red);
146  ++jter;
147  }
148  view1_->update();
149  }
150 
151 private:
152  ImageView * view1_;
153 };
154 
155 std::multimap<int, cv::KeyPoint> aggregate(const std::list<int> & wordIds, const std::vector<cv::KeyPoint> & keypoints)
156 {
157  std::multimap<int, cv::KeyPoint> words;
158  std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
159  for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
160  {
161  words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
162  ++kpIter;
163  }
164  return words;
165 }
166 
167 
168 
169 int main(int argc, char** argv)
170 {
171  ULogger::setType(ULogger::kTypeConsole);
172  ULogger::setLevel(ULogger::kInfo);
173 
174  cv::Mat image1;
175  cv::Mat image2;
176  if(argc == 3)
177  {
178  image1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
179  image2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
180  }
181  else
182  {
183  showUsage();
184  }
185 
186  QTime timer;
187  timer.start();
188 
189  // Extract words
190  timer.start();
191  VWDictionary dictionary;
192  ParametersMap param;
193  param.insert(ParametersPair(Parameters::kSURFExtended(), "true"));
194  param.insert(ParametersPair(Parameters::kSURFHessianThreshold(), "100"));
195  SURF detector(param);
196  std::vector<cv::KeyPoint> kpts1 = detector.generateKeypoints(image1);
197  std::vector<cv::KeyPoint> kpts2 = detector.generateKeypoints(image2);
198  cv::Mat descriptors1 = detector.generateDescriptors(image1, kpts1);
199  cv::Mat descriptors2 = detector.generateDescriptors(image2, kpts2);
200  UINFO("detect/extract features = %d ms", timer.elapsed());
201 
202  timer.start();
203  std::list<int> wordIds1 = dictionary.addNewWords(descriptors1, 1);
204  dictionary.update();
205  std::list<int> wordIds2 = dictionary.addNewWords(descriptors2, 2);
206  UINFO("quantization to words = %d ms", timer.elapsed());
207 
208  std::multimap<int, cv::KeyPoint> words1 = aggregate(wordIds1, kpts1);
209  std::multimap<int, cv::KeyPoint> words2 = aggregate(wordIds2, kpts2);
210 
211  // Find pairs
212  timer.start();
213  std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
214  EpipolarGeometry::findPairsUnique(words1, words2, pairs);
215  UINFO("find pairs = %d ms", timer.elapsed());
216 
217  // Find fundamental matrix
218  timer.start();
219  std::vector<uchar> status;
220  cv::Mat fundamentalMatrix = EpipolarGeometry::findFFromWords(pairs, status);
221  UINFO("inliers = %d/%d", uSum(status), pairs.size());
222  UINFO("find F = %d ms", timer.elapsed());
223  if(!fundamentalMatrix.empty())
224  {
225  int i = 0;
226  int goodCount = 0;
227  for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
228  {
229  if(status[i])
230  {
231  // the output of the correspondences can be easily copied in MatLab
232  if(goodCount==0)
233  {
234  printf("x=[%f %f %d]; xp=[%f %f %d];\n",
235  iter->second.first.pt.x,
236  iter->second.first.pt.y,
237  iter->first,
238  iter->second.second.pt.x,
239  iter->second.second.pt.y,
240  iter->first);
241  }
242  else
243  {
244  printf("x=[x;[%f %f %d]]; xp=[xp;[%f %f %d]];\n",
245  iter->second.first.pt.x,
246  iter->second.first.pt.y,
247  iter->first,
248  iter->second.second.pt.x,
249  iter->second.second.pt.y,
250  iter->first);
251  }
252  ++goodCount;
253  }
254  ++i;
255  }
256 
257  // Show the fundamental matrix
258  std::cout << "F=" << fundamentalMatrix << std::endl;
259 
260  // Intrinsic parameters K of the camera (guest... non-calibrated camera)
261  cv::Mat k = cv::Mat::zeros(3,3,CV_64FC1);
262  k.at<double>(0,0) = image1.cols; // focal x
263  k.at<double>(1,1) = image1.rows; // focal y
264  k.at<double>(2,2) = 1;
265  k.at<double>(0,2) = image1.cols/2; // center x in pixels
266  k.at<double>(1,2) = image1.rows/2; // center y in pixels
267 
268  // Use essential matrix E=K'*F*K
269 
270  cv::Mat e = k.t()*fundamentalMatrix*k;
271 
272  //remove K from points xe = inv(K)*x
273  cv::Mat x1(2, goodCount, CV_64FC1);
274  cv::Mat x2(2, goodCount, CV_64FC1);
275  i=0;
276  int j=0;
277  cv::Mat invK = k.inv();
278  for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin(); iter!=pairs.end(); ++iter)
279  {
280  if(status[i])
281  {
282  cv::Mat tmp(3,1,CV_64FC1);
283  tmp.at<double>(0,0) = iter->second.first.pt.x;
284  tmp.at<double>(1,0) = iter->second.first.pt.y;
285  tmp.at<double>(2,0) = 1;
286  tmp = invK*tmp;
287  x1.at<double>(0,j) = tmp.at<double>(0,0);
288  x1.at<double>(1,j) = tmp.at<double>(1,0);
289  tmp.at<double>(0,0) = iter->second.second.pt.x;
290  tmp.at<double>(1,0) = iter->second.second.pt.y;
291  tmp.at<double>(2,0) = 1;
292  tmp = invK*tmp;
293  x2.at<double>(0,j) = tmp.at<double>(0,0);
294  x2.at<double>(1,j) = tmp.at<double>(1,0);
295  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));
296  ++j;
297  }
298  ++i;
299  }
300 
301  std::cout<<"K=" << k << std::endl;
302  timer.start();
303  //std::cout<<"e=" << e << std::endl;
304  cv::Mat p = EpipolarGeometry::findPFromE(e, x1, x2);
305  cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
306  p0.at<double>(0,0) = 1;
307  p0.at<double>(1,1) = 1;
308  p0.at<double>(2,2) = 1;
309  UINFO("find P from F = %d ms", timer.elapsed());
310 
311  std::cout<<"P=" << p << std::endl;
312 
313  //find 4D homogeneous points
314  cv::Mat x4d;
315  timer.start();
316  cv::triangulatePoints(p0, p, x1, x2, x4d);
317  UINFO("find X (triangulate) = %d ms", timer.elapsed());
318 
319  //Show 4D points
320  for(int i=0; i<x4d.cols; ++i)
321  {
322  x4d.at<double>(0,i) = x4d.at<double>(0,i)/x4d.at<double>(3,i);
323  x4d.at<double>(1,i) = x4d.at<double>(1,i)/x4d.at<double>(3,i);
324  x4d.at<double>(2,i) = x4d.at<double>(2,i)/x4d.at<double>(3,i);
325  x4d.at<double>(3,i) = x4d.at<double>(3,i)/x4d.at<double>(3,i);
326  if(i==0)
327  {
328  printf("X=[%f;%f;%f;%f];\n",
329  x4d.at<double>(0,i),
330  x4d.at<double>(1,i),
331  x4d.at<double>(2,i),
332  x4d.at<double>(3,i));
333  }
334  else
335  {
336  printf("X=[X [%f;%f;%f;%f]];\n",
337  x4d.at<double>(0,i),
338  x4d.at<double>(1,i),
339  x4d.at<double>(2,i),
340  x4d.at<double>(3,i));
341  }
342  }
343 
344  //Show rotation/translation of the second camera
345  cv::Mat r;
346  cv::Mat t;
347  EpipolarGeometry::findRTFromP(p, r, t);
348  std::cout<< "R=" << r << std::endl;
349  std::cout<< "t=" << t << std::endl;
350 
351  //GUI
352  QApplication app(argc, argv);
353  MainWidget mainWidget(image1, image2, words1, words2, status);
354  mainWidget.show();
355  app.exec();
356  }
357  else
358  {
359  UINFO("Fundamental matrix not found...");
360  }
361 
362  return 0;
363 }
UTimer
Definition: UTimer.h:46
rtabmap::VWDictionary::update
virtual void update()
Definition: VWDictionary.cpp:468
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
aggregate
std::multimap< int, cv::KeyPoint > aggregate(const std::list< int > &wordIds, const std::vector< cv::KeyPoint > &keypoints)
Definition: tools/EpipolarGeometry/main.cpp:155
MainWidget::drawKeypoints
void drawKeypoints(const std::multimap< int, cv::KeyPoint > &refWords, const std::multimap< int, cv::KeyPoint > &loopWords, const std::vector< uchar > &status)
Definition: tools/EpipolarGeometry/main.cpp:96
MainWidget::MainWidget
MainWidget(const cv::Mat &image1, const cv::Mat &image2, const std::multimap< int, cv::KeyPoint > &words1, const std::multimap< int, cv::KeyPoint > &words2, const std::vector< uchar > &status)
Definition: tools/EpipolarGeometry/main.cpp:69
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
glm::e
GLM_FUNC_DECL genType e()
rtabmap::VWDictionary::addNewWords
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
Definition: VWDictionary.cpp:759
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UMath.h
Basic mathematics functions.
UConversion.h
Some conversion functions.
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:670
rtabmap::EpipolarGeometry::findFFromWords
static cv::Mat findFFromWords(const std::list< std::pair< int, std::pair< cv::KeyPoint, cv::KeyPoint > > > &pairs, std::vector< uchar > &status, double ransacReprojThreshold=3.0, double ransacConfidence=0.99)
Definition: EpipolarGeometry.cpp:298
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
main
int main(int argc, char **argv)
Definition: tools/EpipolarGeometry/main.cpp:169
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
MainWidget::showEvent
virtual void showEvent(QShowEvent *event)
Definition: tools/EpipolarGeometry/main.cpp:91
UTimer::start
void start()
Definition: UTimer.cpp:87
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:59
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::Feature2D::generateDescriptors
cv::Mat generateDescriptors(const cv::Mat &image, std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:772
UDEBUG
#define UDEBUG(...)
rtabmap::EpipolarGeometry::findPairsUnique
static int findPairsUnique(const std::multimap< int, T > &wordsA, const std::multimap< int, T > &wordsB, std::list< std::pair< int, std::pair< T, T > > > &pairs, bool ignoreNegativeIds=true)
Definition: EpipolarGeometry.h:158
rtabmap_oanet.argv
argv
Definition: rtabmap_oanet.py:12
ULogger.h
ULogger class and convenient macros.
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::EpipolarGeometry::findRTFromP
static void findRTFromP(const cv::Mat &p, cv::Mat &r, cv::Mat &t)
Definition: EpipolarGeometry.cpp:381
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:320
rtabmap::EpipolarGeometry::findPFromE
static cv::Mat findPFromE(const cv::Mat &E, const cv::Mat &x, const cv::Mat &xp)
Definition: EpipolarGeometry.cpp:185
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29