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 <QtCore/QElapsedTimer>
55 #include <QGraphicsEffect>
56 
57 
58 using namespace rtabmap;
59 
60 void showUsage()
61 {
62  printf("\nUsage:\n"
63  "rtabmap-epipolar_geometry image1.jpg image2.jpg\n");
64  exit(1);
65 }
66 
67 class MainWidget : public QWidget
68 {
69 public:
70  MainWidget(const cv::Mat & image1,
71  const cv::Mat & image2,
72  const std::multimap<int, cv::KeyPoint> & words1,
73  const std::multimap<int, cv::KeyPoint> & words2,
74  const std::vector<uchar> & status)
75  {
76  view1_ = new ImageView(this);
77  this->setLayout(new QHBoxLayout());
78  this->layout()->setSpacing(0);
79  this->layout()->setContentsMargins(0,0,0,0);
80  this->layout()->addWidget(view1_);
81  view1_->setSceneRect(QRectF(0,0,(float)image1.cols, (float)image1.rows));
82  view1_->setLinesShown(true);
83  view1_->setFeaturesShown(false);
84  view1_->setImageDepthShown(true);
85 
86  view1_->setImage(uCvMat2QImage(image1));
87  view1_->setImageDepth(image2);
88 
89  drawKeypoints(words1, words2, status);
90  }
91 protected:
92  virtual void showEvent(QShowEvent* event)
93  {
94  resizeEvent(0);
95  }
96 private:
97  void drawKeypoints(const std::multimap<int, cv::KeyPoint> & refWords, const std::multimap<int, cv::KeyPoint> & loopWords, const std::vector<uchar> & status)
98  {
99  UTimer timer;
100 
101  timer.start();
102  QList<QPair<cv::Point2f, cv::Point2f> > uniqueCorrespondences;
103  QList<bool> inliers;
104  int j=0;
105  for(std::multimap<int, cv::KeyPoint>::const_iterator i = refWords.begin(); i != refWords.end(); ++i )
106  {
107  int id = (*i).first;
108  QColor color;
109  if(uContains(loopWords, id))
110  {
111  // PINK = FOUND IN LOOP SIGNATURE
112  color = Qt::magenta;
113  //To draw lines... get only unique correspondences
114  if(uValues(refWords, id).size() == 1 && uValues(loopWords, id).size() == 1)
115  {
116  uniqueCorrespondences.push_back(QPair<cv::Point2f, cv::Point2f>(i->second.pt, uValues(loopWords, id).begin()->pt));
117  inliers.push_back(status[j++]);
118  }
119  }
120  else if(refWords.count(id) > 1)
121  {
122  // YELLOW = NEW and multiple times
123  color = Qt::yellow;
124  }
125  else
126  {
127  // GREEN = NEW
128  color = Qt::green;
129  }
130  view1_->addFeature(id, i->second, 0, color);
131  }
132  ULOGGER_DEBUG("source time = %f s", timer.ticks());
133 
134  // Draw lines between corresponding features...
135  UASSERT(uniqueCorrespondences.size() == inliers.size());
136  QList<bool>::iterator jter = inliers.begin();
137  for(QList<QPair<cv::Point2f, cv::Point2f> >::iterator iter = uniqueCorrespondences.begin();
138  iter!=uniqueCorrespondences.end();
139  ++iter)
140  {
141  view1_->addLine(
142  iter->first.x,
143  iter->first.y,
144  iter->second.x,
145  iter->second.y,
146  *jter?Qt::cyan:Qt::red);
147  ++jter;
148  }
149  view1_->update();
150  }
151 
152 private:
153  ImageView * view1_;
154 };
155 
156 std::multimap<int, cv::KeyPoint> aggregate(const std::list<int> & wordIds, const std::vector<cv::KeyPoint> & keypoints)
157 {
158  std::multimap<int, cv::KeyPoint> words;
159  std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
160  for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
161  {
162  words.insert(std::pair<int, cv::KeyPoint >(*iter, *kpIter));
163  ++kpIter;
164  }
165  return words;
166 }
167 
168 
169 
170 int main(int argc, char** argv)
171 {
172  ULogger::setType(ULogger::kTypeConsole);
173  ULogger::setLevel(ULogger::kInfo);
174 
175  cv::Mat image1;
176  cv::Mat image2;
177  if(argc == 3)
178  {
179  image1 = cv::imread(argv[1], cv::IMREAD_GRAYSCALE);
180  image2 = cv::imread(argv[2], cv::IMREAD_GRAYSCALE);
181  }
182  else
183  {
184  showUsage();
185  }
186 
187  QElapsedTimer timer;
188  timer.start();
189 
190  // Extract words
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 }
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
UINFO
#define UINFO(...)
ImageView.h
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:728
MainWidget::showEvent
virtual void showEvent(QShowEvent *event)
Definition: tools/EpipolarGeometry/main.cpp:92
x1
x1
timer
uSum
T uSum(const std::list< T > &list)
Definition: UMath.h:318
VWDictionary.h
UDirectory.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
size
Index size
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::SURF
Definition: Features2d.h:252
aggregate
std::multimap< int, cv::KeyPoint > aggregate(const std::list< int > &wordIds, const std::vector< cv::KeyPoint > &keypoints)
Definition: tools/EpipolarGeometry/main.cpp:156
iterator
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
EpipolarGeometry.h
rtabmap::Feature2D::generateDescriptors
cv::Mat generateDescriptors(const cv::Mat &image, std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:833
rtabmap::VWDictionary::update
virtual void update()
Definition: VWDictionary.cpp:468
KeypointItem.h
UMath.h
Basic mathematics functions.
main
int main(int argc, char **argv)
Definition: tools/EpipolarGeometry/main.cpp:170
Odometry.h
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
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
rtabmap::VWDictionary::addNewWords
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
Definition: VWDictionary.cpp:759
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:59
rtabmap::EpipolarGeometry::findPFromE
static cv::Mat findPFromE(const cv::Mat &E, const cv::Mat &x, const cv::Mat &xp)
Definition: EpipolarGeometry.cpp:185
Features2d.h
j
std::ptrdiff_t j
UConversion.h
Some conversion functions.
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
MainWidget
Definition: tools/EpipolarGeometry/main.cpp:67
UCv2Qt.h
x2
x2
ULogger::kInfo
@ kInfo
Definition: ULogger.h:252
UASSERT
#define UASSERT(condition)
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
p
Point3_ p(2)
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
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:97
timer::elapsed
double elapsed() const
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
ULogger.h
ULogger class and convenient macros.
rtabmap::VWDictionary
Definition: VWDictionary.h:46
iter
iterator iter(handle obj)
p0
Vector3f p0
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:70
rtabmap::EpipolarGeometry::findRTFromP
static void findRTFromP(const cv::Mat &p, cv::Mat &r, cv::Mat &t)
Definition: EpipolarGeometry.cpp:381
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
UTimer
Definition: UTimer.h:46
rtabmap::ImageView
Definition: ImageView.h:49
showUsage
void showUsage()
Definition: tools/EpipolarGeometry/main.cpp:60
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
MainWidget::view1_
ImageView * view1_
Definition: tools/EpipolarGeometry/main.cpp:153
i
int i

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:12