tools/Matcher/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 <rtabmap/core/Parameters.h>
29 #include <rtabmap/core/RegistrationVis.h>
30 #include <rtabmap/core/EpipolarGeometry.h>
31 #include <rtabmap/core/Features2d.h>
32 #include <rtabmap/core/util3d.h>
33 #include <rtabmap/core/VWDictionary.h>
34 #include <rtabmap/core/util3d_filtering.h>
35 #include <rtabmap/core/util3d_features.h>
36 #include <rtabmap/utilite/ULogger.h>
37 #include <rtabmap/gui/ImageView.h>
38 #include <rtabmap/gui/KeypointItem.h>
39 #include <rtabmap/gui/CloudViewer.h>
40 #include <rtabmap/utilite/UCv2Qt.h>
41 #include <rtabmap/utilite/UDirectory.h>
42 #include <rtabmap/utilite/UFile.h>
43 #include <rtabmap/utilite/UStl.h>
44 #include <rtabmap/utilite/UTimer.h>
45 #ifdef RTABMAP_PYTHON
46 #include <rtabmap/core/PythonInterface.h>
47 #endif
48 #include <fstream>
49 #include <string>
50 #include <QApplication>
51 #include <QDialog>
52 #include <QHBoxLayout>
53 #include <QMultiMap>
54 #include <QString>
55 #include <opencv2/core/core.hpp>
56 
57 using namespace rtabmap;
58 
59 void showUsage()
60 {
61  printf("\n\nUsage:\n"
62  " rtabmap-matcher [Options] from.png to.png\n"
63  "Examples:\n"
64  " rtabmap-matcher --Vis/CorNNType 5 --Vis/PnPReprojError 3 from.png to.png\n"
65  " rtabmap-matcher --Vis/CorNNDR 0.8 from.png to.png\n"
66  " rtabmap-matcher --Vis/FeatureType 11 --SuperPoint/ModelPath \"superpoint.pt\" --Vis/CorNNType 6 --PyMatcher/Path \"~/SuperGluePretrainedNetwork/rtabmap_superglue.py\" from.png to.png\n"
67  " rtabmap-matcher --Vis/FeatureType 1 --Vis/CorNNType 6 --PyMatcher/Path \"~/OANet/demo/rtabmap_oanet.py\" --PyMatcher/Model \"~/OANet/model/gl3d/sift-4000/model_best.pth\" from.png to.png\n"
68  " rtabmap-matcher --calibration calib.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
69  " rtabmap-matcher --calibration calibFrom.yaml --calibration_to calibTo.yaml --from_depth from_depth.png --to_depth to_depth.png from.png to.png\n"
70  " rtabmap-matcher --calibration calib.yaml --Vis/FeatureType 2 --Vis/MaxFeatures 10000 --Vis/CorNNType 7 from.png to.png\n"
71  "\n"
72  "Note: Use \"Vis/\" parameters for feature stuff.\n"
73  "Options:\n"
74  " --calibration \"calibration.yaml\" Calibration file. If not set, a\n"
75  " fake one is created from image's\n"
76  " size (which may not be optimal).\n"
77  " Required if from_depth option is set.\n"
78  " Assuming same calibration for both images\n"
79  " if --calibration_to is not set.\n"
80  " --calibration_to \"calibration.yaml\" Calibration file for \"to\" image. If not set,\n"
81  " the same calibration of --calibration option is\n"
82  " used for \"to\" image.\n"
83  " --from_depth \"from_depth.png\" Depth or right image file of the first image.\n"
84  " If not set, 2D->2D estimation is done by \n"
85  " default. For 3D->2D estimation, from_depth\n"
86  " should be set.\n"
87  " --to_depth \"to_depth.png\" Depth or right image file of the second image.\n"
88  " For 3D->3D estimation, from_depth and to_depth\n"
89  " should be both set.\n"
90  "\n\n"
91  "%s\n",
92  Parameters::showUsage());
93  exit(1);
94 }
95 
96 int main(int argc, char * argv[])
97 {
98  if(argc < 3)
99  {
100  showUsage();
101  }
102 
103  ULogger::setLevel(ULogger::kWarning);
104  ULogger::setType(ULogger::kTypeConsole);
105 
106  std::string fromDepthPath;
107  std::string toDepthPath;
108  std::string calibrationPath;
109  std::string calibrationToPath;
110  for(int i=1; i<argc-2; ++i)
111  {
112  if(strcmp(argv[i], "--from_depth") == 0)
113  {
114  ++i;
115  if(i<argc-2)
116  {
117  fromDepthPath = argv[i];
118  }
119  else
120  {
121  showUsage();
122  }
123  }
124  else if(strcmp(argv[i], "--to_depth") == 0)
125  {
126  ++i;
127  if(i<argc-2)
128  {
129  toDepthPath = argv[i];
130  }
131  else
132  {
133  showUsage();
134  }
135  }
136  else if(strcmp(argv[i], "--calibration") == 0)
137  {
138  ++i;
139  if(i<argc-2)
140  {
141  calibrationPath = argv[i];
142  }
143  else
144  {
145  showUsage();
146  }
147  }
148  else if(strcmp(argv[i], "--calibration_to") == 0)
149  {
150  ++i;
151  if(i<argc-2)
152  {
153  calibrationToPath = argv[i];
154  }
155  else
156  {
157  showUsage();
158  }
159  }
160  else if(strcmp(argv[i], "--help") == 0)
161  {
162  showUsage();
163  }
164  }
165 
166  printf("Options\n");
167  printf(" --calibration = \"%s\"\n", calibrationPath.c_str());
168  if(!calibrationToPath.empty())
169  {
170  printf(" --calibration_to = \"%s\"\n", calibrationToPath.c_str());
171  }
172  printf(" --from_depth = \"%s\"\n", fromDepthPath.c_str());
173  printf(" --to_depth = \"%s\"\n", toDepthPath.c_str());
174 
175 #ifdef RTABMAP_PYTHON
176  rtabmap::PythonInterface pythonInterface;
177 #endif
178 
179  ParametersMap parameters = Parameters::parseArguments(argc, argv);
180  parameters.insert(ParametersPair(Parameters::kRegRepeatOnce(), "false"));
181 
182  cv::Mat imageFrom = cv::imread(argv[argc-2], cv::IMREAD_COLOR);
183  cv::Mat imageTo = cv::imread(argv[argc-1], cv::IMREAD_COLOR);
184 
185  if(!imageFrom.empty() && !imageTo.empty())
186  {
188  // Load data
190 
191  cv::Mat fromDepth;
192  cv::Mat toDepth;
193  if(!calibrationPath.empty())
194  {
195  if(!fromDepthPath.empty())
196  {
197  fromDepth = cv::imread(fromDepthPath, cv::IMREAD_UNCHANGED);
198  if(fromDepth.type() == CV_8UC3)
199  {
200  cv::cvtColor(fromDepth, fromDepth, cv::COLOR_BGR2GRAY);
201  }
202  else if(fromDepth.empty())
203  {
204  printf("Failed loading from_depth image: \"%s\"!", fromDepthPath.c_str());
205  }
206  }
207  if(!toDepthPath.empty())
208  {
209  toDepth = cv::imread(toDepthPath, cv::IMREAD_UNCHANGED);
210  if(toDepth.type() == CV_8UC3)
211  {
212  cv::cvtColor(toDepth, toDepth, cv::COLOR_BGR2GRAY);
213  }
214  else if(toDepth.empty())
215  {
216  printf("Failed loading to_depth image: \"%s\"!", toDepthPath.c_str());
217  }
218  }
219  UASSERT(toDepth.empty() || (!fromDepth.empty() && fromDepth.type() == toDepth.type()));
220  }
221  else if(!fromDepthPath.empty() || !fromDepthPath.empty())
222  {
223  printf("A calibration file should be provided if depth images are used!\n");
224  showUsage();
225  }
226 
227  CameraModel model;
228  StereoCameraModel stereoModel;
229  CameraModel modelTo;
230  StereoCameraModel stereoModelTo;
231  if(!fromDepth.empty())
232  {
233  if(fromDepth.type() != CV_8UC1)
234  {
235  if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
236  {
237  printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
238  exit(-1);
239  }
240  if(calibrationToPath.empty())
241  {
242  modelTo = model;
243  }
244  }
245  else // fromDepth.type() == CV_8UC1
246  {
247  if(!stereoModel.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
248  {
249  printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
250  exit(-1);
251  }
252  if(calibrationToPath.empty())
253  {
254  stereoModelTo = stereoModel;
255  }
256  }
257  if(!calibrationToPath.empty())
258  {
259  if(toDepth.empty() || toDepth.type() != CV_8UC1)
260  {
261  if(!modelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
262  {
263  printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
264  exit(-1);
265  }
266  }
267  else // toDepth.type() == CV_8UC1
268  {
269  if(!stereoModelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
270  {
271  printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
272  exit(-1);
273  }
274  }
275  }
276  }
277  else if(!calibrationPath.empty())
278  {
279  if(!model.load(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front()))
280  {
281  printf("Failed to load calibration file \"%s\"!\n", calibrationPath.c_str());
282  exit(-1);
283  }
284  if(!calibrationToPath.empty())
285  {
286  if(!modelTo.load(UDirectory::getDir(calibrationToPath), uSplit(UFile::getName(calibrationToPath), '.').front()))
287  {
288  printf("Failed to load calibration file \"%s\"!\n", calibrationToPath.c_str());
289  exit(-1);
290  }
291  }
292  else
293  {
294  modelTo = model;
295  }
296  }
297  else
298  {
299  printf("Using fake calibration model \"from\" (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
300  imageFrom.cols, imageFrom.rows, imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2);
301  model = CameraModel(imageFrom.cols/2, imageFrom.cols/2, imageFrom.cols/2, imageFrom.rows/2); // Fake model
302  model.setImageSize(imageFrom.size());
303  printf("Using fake calibration model \"to\" (image size=%dx%d): fx=%d fy=%d cx=%d cy=%d\n",
304  imageTo.cols, imageTo.rows, imageTo.cols/2, imageTo.cols/2, imageTo.cols/2, imageTo.rows/2);
305  modelTo = CameraModel(imageTo.cols/2, imageTo.cols/2, imageTo.cols/2, imageTo.rows/2); // Fake model
306  modelTo.setImageSize(imageTo.size());
307  }
308 
309  Signature dataFrom;
310  Signature dataTo;
311  if(model.isValidForProjection())
312  {
313  printf("Mono calibration model detected.\n");
314  dataFrom = SensorData(imageFrom, fromDepth, model, 1);
315  dataTo = SensorData(imageTo, toDepth, modelTo, 2);
316  }
317  else //stereo
318  {
319  printf("Stereo calibration model detected.\n");
320  dataFrom = SensorData(imageFrom, fromDepth, stereoModel, 1);
321  dataTo = SensorData(imageTo, toDepth, stereoModelTo, 2);
322  }
323 
325  // Registration
327 
328  if(fromDepth.empty())
329  {
330  parameters.insert(ParametersPair(Parameters::kVisEstimationType(), "2")); // Set 2D->2D estimation for mono images
331  parameters.insert(ParametersPair(Parameters::kVisEpipolarGeometryVar(), "1")); //Unknown scale
332  printf("Calibration not set, setting %s=1 and %s=2 by default (2D->2D estimation)\n", Parameters::kVisEpipolarGeometryVar().c_str(), Parameters::kVisEstimationType().c_str());
333  }
334  RegistrationVis reg(parameters);
335  RegistrationInfo info;
336 
337 
338  // Do it one time before to make sure everything is loaded to get realistic timing for matching only.
339  reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
340 
341  UTimer timer;
342  Transform t = reg.computeTransformationMod(dataFrom, dataTo, Transform(), &info);
343  double matchingTime = timer.ticks();
344  printf("Time matching and motion estimation (excluding feature detection): %fs\n", matchingTime);
345 
347  // Visualization
349 
350  if(reg.getNNType()==6 &&
351  !dataFrom.getWordsDescriptors().empty() &&
352  dataFrom.getWordsDescriptors().type()!=CV_32F)
353  {
354  UWARN("PyMatcher is selected for matching but binary features "
355  "are not compatible. BruteForce with CrossCheck (%s=5) "
356  "has been used instead.", Parameters::kVisCorNNType().c_str());
357  }
358 
359  QApplication app(argc, argv);
360  QDialog dialog;
361  float reprojError = Parameters::defaultVisPnPReprojError();
362  std::string pyMatcherPath;
363  Parameters::parse(parameters, Parameters::kVisPnPReprojError(), reprojError);
364  Parameters::parse(parameters, Parameters::kPyMatcherPath(), pyMatcherPath);
365  dialog.setWindowTitle(QString("Matches (%1/%2) %3 sec [%4=%5 (%6) %7=%8 (%9)%10 %11=%12 (%13) %14=%15]")
366  .arg(info.inliers)
367  .arg(info.matches)
368  .arg(matchingTime)
369  .arg(Parameters::kVisFeatureType().c_str())
370  .arg(reg.getDetector()?reg.getDetector()->getType():-1)
371  .arg(reg.getDetector()?Feature2D::typeName(reg.getDetector()->getType()).c_str():"?")
372  .arg(Parameters::kVisCorNNType().c_str())
373  .arg(reg.getNNType())
374  .arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():
375  reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().type()!=CV_32F)?"BFCrossCheck":
376  reg.getNNType()==6?QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", ""):
377  reg.getNNType()==7?"GMS":"?")
378  .arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
379  .arg(Parameters::kVisEstimationType().c_str())
380  .arg(reg.getEstimationType())
381  .arg(reg.getEstimationType()==0?"3D->3D":reg.getEstimationType()==1?"3D->2D":reg.getEstimationType()==2?"2D->2D":"?")
382  .arg(Parameters::kVisPnPReprojError().c_str())
383  .arg(reprojError));
384 
385  CloudViewer * viewer = 0;
386  if(!t.isNull())
387  {
388  viewer = new CloudViewer(&dialog);
389  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom = util3d::cloudRGBFromSensorData(dataFrom.sensorData());
390  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo = util3d::cloudRGBFromSensorData(dataTo.sensorData());
391  viewer->addCloud(uFormat("cloud_%d", dataFrom.id()), cloudFrom, Transform::getIdentity(), Qt::magenta);
392  viewer->addCloud(uFormat("cloud_%d", dataTo.id()), cloudTo, t, Qt::cyan);
393  viewer->addOrUpdateCoordinate(uFormat("frame_%d", dataTo.id()), t, 0.2);
394  viewer->setGridShown(true);
395 
396  if(reg.getEstimationType() == 2)
397  {
398  // triangulate 3D words based on the transform computed
399  std::map<int, int> wordsFrom = uMultimapToMapUnique(dataFrom.getWords());
400  std::map<int, int> wordsTo = uMultimapToMapUnique(dataTo.getWords());
401  std::map<int, cv::KeyPoint> kptsFrom;
402  std::map<int, cv::KeyPoint> kptsTo;
403  for(std::map<int, int>::iterator iter=wordsFrom.begin(); iter!=wordsFrom.end(); ++iter)
404  {
405  kptsFrom.insert(std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
406  }
407  for(std::map<int, int>::iterator iter=wordsTo.begin(); iter!=wordsTo.end(); ++iter)
408  {
409  kptsTo.insert(std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
410  }
411  std::map<int, cv::Point3f> points3d = util3d::generateWords3DMono(
412  kptsFrom,
413  kptsTo,
414  model.isValidForProjection()?model:stereoModel.left(),
415  t);
416  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
417  cloudWordsFrom->resize(points3d.size());
418  int i=0;
419  for(std::multimap<int, cv::Point3f>::const_iterator iter=points3d.begin();
420  iter!=points3d.end();
421  ++iter)
422  {
423  cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
424  }
425  if(cloudWordsFrom->size())
426  {
427  cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
428  }
429  if(cloudWordsFrom->size())
430  {
431  viewer->addCloud("wordsFrom", cloudWordsFrom, Transform::getIdentity(), Qt::yellow);
432  viewer->setCloudPointSize("wordsFrom", 5);
433  }
434  }
435  else
436  {
437  if(!dataFrom.getWords3().empty())
438  {
439  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
440  cloudWordsFrom->resize(dataFrom.getWords3().size());
441  int i=0;
442  for(std::multimap<int, int>::const_iterator iter=dataFrom.getWords().begin();
443  iter!=dataFrom.getWords().end();
444  ++iter)
445  {
446  const cv::Point3f & pt = dataFrom.getWords3()[iter->second];
447  cloudWordsFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
448  }
449  if(cloudWordsFrom->size())
450  {
451  cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
452  }
453  if(cloudWordsFrom->size())
454  {
455  viewer->addCloud("wordsFrom", cloudWordsFrom, Transform::getIdentity(), Qt::magenta);
456  viewer->setCloudPointSize("wordsFrom", 5);
457  }
458  }
459  if(!dataTo.getWords3().empty())
460  {
461  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsTo(new pcl::PointCloud<pcl::PointXYZ>);
462  cloudWordsTo->resize(dataTo.getWords3().size());
463  int i=0;
464  for(std::multimap<int, int>::const_iterator iter=dataTo.getWords().begin();
465  iter!=dataTo.getWords().end();
466  ++iter)
467  {
468  const cv::Point3f & pt = dataTo.getWords3()[iter->second];
469  cloudWordsTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
470  }
471  if(cloudWordsTo->size())
472  {
473  cloudWordsTo = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsTo);
474  }
475  if(cloudWordsTo->size())
476  {
477  viewer->addCloud("wordsTo", cloudWordsTo, t, Qt::cyan);
478  viewer->setCloudPointSize("wordsTo", 5);
479  }
480  }
481  }
482  }
483 
484  QBoxLayout * mainLayout = new QHBoxLayout();
485  mainLayout->setContentsMargins(0, 0, 0, 0);
486  mainLayout->setSpacing(0);
487  QBoxLayout * layout;
488  bool vertical=true;
489  if(imageFrom.cols > imageFrom.rows)
490  {
491  dialog.setMinimumWidth(640*(viewer?2:1));
492  dialog.setMinimumHeight(640*imageFrom.rows/imageFrom.cols*2);
493  layout = new QVBoxLayout();
494  }
495  else
496  {
497  dialog.setMinimumWidth((640*imageFrom.cols/imageFrom.rows*2)*(viewer?2:1));
498  dialog.setMinimumHeight(640);
499  layout = new QHBoxLayout();
500  vertical = false;
501  }
502 
503  ImageView * viewA = new ImageView(&dialog);
504  ImageView * viewB = new ImageView(&dialog);
505 
506  layout->setSpacing(0);
507  layout->addWidget(viewA, 1);
508  layout->addWidget(viewB, 1);
509 
510  mainLayout->addLayout(layout, 1);
511  if(viewer)
512  {
513  mainLayout->addWidget(viewer, 1);
514  }
515 
516  dialog.setLayout(mainLayout);
517 
518  dialog.show();
519 
520  viewA->setImage(uCvMat2QImage(imageFrom));
521  viewA->setAlpha(200);
522  if(!fromDepth.empty())
523  {
524  viewA->setImageDepth(uCvMat2QImage(fromDepth, false, uCvQtDepthRedToBlue));
525  viewA->setImageDepthShown(true);
526  }
527  viewB->setImage(uCvMat2QImage(imageTo));
528  viewB->setAlpha(200);
529  if(!toDepth.empty())
530  {
531  viewB->setImageDepth(uCvMat2QImage(toDepth, false, uCvQtDepthRedToBlue));
532  viewB->setImageDepthShown(true);
533  }
534  std::multimap<int, cv::KeyPoint> keypointsFrom;
535  std::multimap<int, cv::KeyPoint> keypointsTo;
536  if(!dataFrom.getWordsKpts().empty())
537  {
538  for(std::map<int, int>::const_iterator iter=dataFrom.getWords().begin(); iter!=dataFrom.getWords().end(); ++iter)
539  {
540  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
541  }
542  }
543  if(!dataTo.getWordsKpts().empty())
544  {
545  for(std::map<int, int>::const_iterator iter=dataTo.getWords().begin(); iter!=dataTo.getWords().end(); ++iter)
546  {
547  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
548  }
549  }
550  viewA->setFeatures(keypointsFrom);
551  viewB->setFeatures(keypointsTo);
552  std::set<int> inliersSet(info.inliersIDs.begin(), info.inliersIDs.end());
553 
554  const QMultiMap<int, KeypointItem*> & wordsA = viewA->getFeatures();
555  const QMultiMap<int, KeypointItem*> & wordsB = viewB->getFeatures();
556  if(wordsA.size() && wordsB.size())
557  {
558  QList<int> ids = wordsA.uniqueKeys();
559  for(int i=0; i<ids.size(); ++i)
560  {
561  if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
562  {
563  // Add lines
564  // Draw lines between corresponding features...
565  float scaleAX = viewA->viewScale();
566  float scaleBX = viewB->viewScale();
567 
568  float scaleDiff = viewA->viewScale() / viewB->viewScale();
569  float deltaAX = 0;
570  float deltaAY = 0;
571 
572  if(vertical)
573  {
574  deltaAY = viewA->height()/scaleAX;
575  }
576  else
577  {
578  deltaAX = viewA->width()/scaleAX;
579  }
580 
581  float deltaBX = 0;
582  float deltaBY = 0;
583 
584  if(vertical)
585  {
586  deltaBY = viewB->height()/scaleBX;
587  }
588  else
589  {
590  deltaBX = viewA->width()/scaleBX;
591  }
592 
593  const KeypointItem * kptA = wordsA.value(ids[i]);
594  const KeypointItem * kptB = wordsB.value(ids[i]);
595 
596  QColor cA = viewA->getDefaultMatchingLineColor();
597  QColor cB = viewB->getDefaultMatchingLineColor();
598  if(inliersSet.find(ids[i])!=inliersSet.end())
599  {
600  cA = viewA->getDefaultMatchingFeatureColor();
601  cB = viewB->getDefaultMatchingFeatureColor();
602  viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingFeatureColor());
603  viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingFeatureColor());
604  }
605  else
606  {
607  viewA->setFeatureColor(ids[i], viewA->getDefaultMatchingLineColor());
608  viewB->setFeatureColor(ids[i], viewB->getDefaultMatchingLineColor());
609  }
610 
611  viewA->addLine(
612  kptA->rect().x()+kptA->rect().width()/2,
613  kptA->rect().y()+kptA->rect().height()/2,
614  kptB->rect().x()/scaleDiff+kptB->rect().width()/scaleDiff/2+deltaAX,
615  kptB->rect().y()/scaleDiff+kptB->rect().height()/scaleDiff/2+deltaAY,
616  cA);
617 
618  viewB->addLine(
619  kptA->rect().x()*scaleDiff+kptA->rect().width()*scaleDiff/2-deltaBX,
620  kptA->rect().y()*scaleDiff+kptA->rect().height()*scaleDiff/2-deltaBY,
621  kptB->rect().x()+kptB->rect().width()/2,
622  kptB->rect().y()+kptB->rect().height()/2,
623  cB);
624  }
625  }
626  viewA->update();
627  viewB->update();
628  }
629 
630  printf("Transform: %s\n", t.prettyPrint().c_str());
631  printf("Features: from=%d to=%d\n", (int)dataFrom.getWords().size(), (int)dataTo.getWords().size());
632  printf("Matches: %d\n", info.matches);
633  printf("Inliers: %d (%s=%d)\n", info.inliers, Parameters::kVisMinInliers().c_str(), reg.getMinInliers());
634  app.exec();
635  delete viewer;
636  }
637  else
638  {
639  printf("Failed loading images %s and %s\n!", argv[argc-2], argv[argc-1]);
640  }
641 
642 
643  return 0;
644 }
645 
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::CameraModel::load
bool load(const std::string &filePath)
Definition: CameraModel.cpp:215
UFile::getName
std::string getName()
Definition: UFile.h:135
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
ImageView.h
rtabmap::ImageView::setImageDepthShown
void setImageDepthShown(bool shown)
Definition: ImageView.cpp:466
rtabmap::RegistrationVis::getEstimationType
int getEstimationType() const
Definition: RegistrationVis.h:59
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
arg::arg
constexpr arg(const char *name=nullptr)
timer
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
Definition: CloudViewer.cpp:745
uCvQtDepthRedToBlue
@ uCvQtDepthRedToBlue
Definition: UCv2Qt.h:33
VWDictionary.h
rtabmap::StereoCameraModel::load
bool load(const std::string &directory, const std::string &cameraName, bool ignoreStereoTransform=true)
Definition: StereoCameraModel.cpp:223
rtabmap::Parameters::parseArguments
static ParametersMap parseArguments(int argc, char *argv[], bool onlyParameters=false)
Definition: Parameters.cpp:599
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
rtabmap::VWDictionary::kNNUndef
@ kNNUndef
Definition: VWDictionary.h:55
rtabmap::RegistrationInfo
Definition: RegistrationInfo.h:34
UDirectory.h
ULogger::kTypeConsole
@ kTypeConsole
Definition: ULogger.h:244
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
rtabmap::RegistrationVis::getNNType
int getNNType() const
Definition: RegistrationVis.h:57
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::ImageView::addLine
void addLine(float x1, float y1, float x2, float y2, QColor color, const QString &text=QString())
Definition: ImageView.cpp:1219
rtabmap::ImageView::viewScale
float viewScale() const
Definition: ImageView.cpp:500
uMultimapToMapUnique
std::map< K, V > uMultimapToMapUnique(const std::multimap< K, V > &m)
Definition: UStl.h:503
showUsage
void showUsage()
Definition: tools/Matcher/main.cpp:59
util3d_features.h
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:116
rtabmap::CameraModel::setImageSize
void setImageSize(const cv::Size &size)
Definition: CameraModel.cpp:189
rtabmap::KeypointItem
Definition: KeypointItem.h:41
util3d.h
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::RegistrationVis::getNNDR
float getNNDR() const
Definition: RegistrationVis.h:58
EpipolarGeometry.h
KeypointItem.h
rtabmap::Feature2D::typeName
static std::string typeName(Type type)
Definition: Features2d.h:126
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
Parameters.h
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
rtabmap::ImageView::setImage
void setImage(const QImage &image, const std::vector< CameraModel > &models=std::vector< CameraModel >(), const Transform &pose=Transform())
Definition: ImageView.cpp:1235
app
QApplication * app
Definition: tools/DataRecorder/main.cpp:59
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
Features2d.h
CloudViewer.h
util3d_filtering.h
rtabmap::PythonInterface
Definition: PythonInterface.h:26
rtabmap::RegistrationVis
Definition: RegistrationVis.h:45
arg
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1735
ULogger::kWarning
@ kWarning
Definition: ULogger.h:252
UCv2Qt.h
rtabmap::RegistrationVis::getMinInliers
int getMinInliers() const
Definition: RegistrationVis.h:56
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:132
rtabmap::util3d::generateWords3DMono
std::map< int, cv::Point3f > RTABMAP_CORE_EXPORT generateWords3DMono(const std::map< int, cv::KeyPoint > &kpts, const std::map< int, cv::KeyPoint > &previousKpts, const CameraModel &cameraModel, Transform &cameraTransform, float ransacReprojThreshold=3.0f, float ransacConfidence=0.99f, const std::map< int, cv::Point3f > &refGuess3D=std::map< int, cv::Point3f >(), double *variance=0, std::vector< int > *matchesOut=0)
Definition: util3d_features.cpp:209
rtabmap::ImageView::getDefaultMatchingFeatureColor
const QColor & getDefaultMatchingFeatureColor() const
Definition: ImageView.cpp:394
info
else if n * info
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
UASSERT
#define UASSERT(condition)
rtabmap_netvlad.argv
argv
Definition: rtabmap_netvlad.py:15
ULogger::setType
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
Definition: ULogger.cpp:176
PythonInterface.h
rtabmap::Feature2D::getType
virtual Feature2D::Type getType() const =0
UWARN
#define UWARN(...)
rtabmap::Parameters::showUsage
static const char * showUsage()
Definition: Parameters.cpp:572
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::ImageView::getFeatures
const QMultiMap< int, rtabmap::KeypointItem * > & getFeatures() const
Definition: ImageView.h:110
rtabmap::VWDictionary::nnStrategyName
static std::string nnStrategyName(NNStrategy strategy)
Definition: VWDictionary.h:58
rtabmap::ImageView::setImageDepth
void setImageDepth(const cv::Mat &imageDepth)
Definition: ImageView.cpp:1264
ULogger.h
ULogger class and convenient macros.
rtabmap::RegistrationVis::getDetector
const Feature2D * getDetector() const
Definition: RegistrationVis.h:61
rtabmap::Transform
Definition: Transform.h:41
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
main
int main(int argc, char *argv[])
Definition: tools/Matcher/main.cpp:96
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
iter
iterator iter(handle obj)
RegistrationVis.h
c_str
const char * c_str(Args &&...args)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::ImageView::getDefaultMatchingLineColor
const QColor & getDefaultMatchingLineColor() const
Definition: ImageView.cpp:398
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:1002
UTimer
Definition: UTimer.h:46
rtabmap::ImageView
Definition: ImageView.h:49
rtabmap::ImageView::setAlpha
void setAlpha(int alpha)
Definition: ImageView.cpp:1343
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::ImageView::setFeatures
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
Definition: ImageView.cpp:1137
rtabmap::VWDictionary::NNStrategy
NNStrategy
Definition: VWDictionary.h:49
t
Point2 t(10, 10)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
trace.model
model
Definition: trace.py:4
i
int i
rtabmap::CloudViewer::setGridShown
void setGridShown(bool shown)
Definition: CloudViewer.cpp:3377
rtabmap::Signature
Definition: Signature.h:48
rtabmap::ImageView::setFeatureColor
void setFeatureColor(int id, QColor color)
Definition: ImageView.cpp:1307

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