RegistrationVis.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 
29 
30 #include <rtabmap/core/RegistrationVis.h>
31 #include <rtabmap/core/util3d_motion_estimation.h>
32 #include <rtabmap/core/util3d_features.h>
33 #include <rtabmap/core/util3d.h>
34 #include <rtabmap/core/VWDictionary.h>
35 #include <rtabmap/core/util2d.h>
36 #include <rtabmap/core/Features2d.h>
37 #include <rtabmap/core/VisualWord.h>
38 #include <rtabmap/core/Optimizer.h>
39 #include <rtabmap/core/util3d_transforms.h>
40 #include <rtabmap/utilite/ULogger.h>
41 #include <rtabmap/utilite/UConversion.h>
42 #include <rtabmap/utilite/UStl.h>
43 #include <rtabmap/utilite/UTimer.h>
44 #include <rtabmap/utilite/UMath.h>
45 #include <opencv2/core/core_c.h>
46 
47 #if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
48 #include <opencv2/xfeatures2d.hpp> // For GMS matcher
49 #endif
50 
51 #include <rtflann/flann.hpp>
52 
53 
54 #ifdef RTABMAP_PYTHON
55  #include "python/PyMatcher.h"
56 #endif
57 
58 namespace rtabmap {
59 
60 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
61  Registration(parameters, child),
62  _minInliers(Parameters::defaultVisMinInliers()),
63  _inlierDistance(Parameters::defaultVisInlierDistance()),
64  _iterations(Parameters::defaultVisIterations()),
65  _refineIterations(Parameters::defaultVisRefineIterations()),
66  _epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
67  _estimationType(Parameters::defaultVisEstimationType()),
68  _forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
69  _PnPReprojError(Parameters::defaultVisPnPReprojError()),
70  _PnPFlags(Parameters::defaultVisPnPFlags()),
71  _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
72  _PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
73  _correspondencesApproach(Parameters::defaultVisCorType()),
74  _flowWinSize(Parameters::defaultVisCorFlowWinSize()),
75  _flowIterations(Parameters::defaultVisCorFlowIterations()),
76  _flowEps(Parameters::defaultVisCorFlowEps()),
77  _flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
78  _nndr(Parameters::defaultVisCorNNDR()),
79  _nnType(Parameters::defaultVisCorNNType()),
80  _gmsWithRotation(Parameters::defaultGMSWithRotation()),
81  _gmsWithScale(Parameters::defaultGMSWithScale()),
82  _gmsThresholdFactor(Parameters::defaultGMSThresholdFactor()),
83  _guessWinSize(Parameters::defaultVisCorGuessWinSize()),
84  _guessMatchToProjection(Parameters::defaultVisCorGuessMatchToProjection()),
85  _bundleAdjustment(Parameters::defaultVisBundleAdjustment()),
86  _depthAsMask(Parameters::defaultVisDepthAsMask()),
87  _minInliersDistributionThr(Parameters::defaultVisMinInliersDistribution()),
88  _maxInliersMeanDistance(Parameters::defaultVisMeanInliersDistance()),
89  _detectorFrom(0),
90  _detectorTo(0)
91 #ifdef RTABMAP_PYTHON
92  ,
93  _pyMatcher(0)
94 #endif
95 {
96  _featureParameters = Parameters::getDefaultParameters();
97  uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
98  uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
99  uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
100  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
101  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
102  uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
103  uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
104  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), _featureParameters.at(Parameters::kVisSubPixWinSize())));
105  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), _featureParameters.at(Parameters::kVisSubPixIterations())));
106  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), _featureParameters.at(Parameters::kVisSubPixEps())));
107  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), _featureParameters.at(Parameters::kVisGridRows())));
108  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), _featureParameters.at(Parameters::kVisGridCols())));
109  uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
110 
111  this->parseParameters(parameters);
112 }
113 
114 void RegistrationVis::parseParameters(const ParametersMap & parameters)
115 {
116  Registration::parseParameters(parameters);
117 
118  Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
119  Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
120  Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
121  Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
122  Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
123  Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
124  Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
125  Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
126  Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
127  Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
128  Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
129  Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
130  Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
131  Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
132  Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
133  Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
134  Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
135  Parameters::parse(parameters, Parameters::kVisCorNNType(), _nnType);
136  Parameters::parse(parameters, Parameters::kGMSWithRotation(), _gmsWithRotation);
137  Parameters::parse(parameters, Parameters::kGMSWithScale(), _gmsWithScale);
138  Parameters::parse(parameters, Parameters::kGMSThresholdFactor(), _gmsThresholdFactor);
139  Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
140  Parameters::parse(parameters, Parameters::kVisCorGuessMatchToProjection(), _guessMatchToProjection);
141  Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
142  Parameters::parse(parameters, Parameters::kVisDepthAsMask(), _depthAsMask);
143  Parameters::parse(parameters, Parameters::kVisMinInliersDistribution(), _minInliersDistributionThr);
144  Parameters::parse(parameters, Parameters::kVisMeanInliersDistance(), _maxInliersMeanDistance);
145  uInsert(_bundleParameters, parameters);
146 
147  if(_minInliers < 6)
148  {
149  UWARN("%s should be >= 6 but it is set to %d, setting to 6.", Parameters::kVisMinInliers().c_str(), _minInliers);
150  _minInliers = 6;
151  }
152  UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
153  UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
154 
155  if(_nnType == 6)
156  {
157  // verify that we have Python3 support
158 #ifndef RTABMAP_PYTHON
159  UWARN("%s is set to 6 but RTAB-Map is not built with Python3 support, using default %d.",
160  Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
161  _nnType = Parameters::defaultVisCorNNType();
162 #else
163  int iterations = _pyMatcher?_pyMatcher->iterations():Parameters::defaultPyMatcherIterations();
164  float matchThr = _pyMatcher?_pyMatcher->matchThreshold():Parameters::defaultPyMatcherThreshold();
165  std::string path = _pyMatcher?_pyMatcher->path():Parameters::defaultPyMatcherPath();
166  bool cuda = _pyMatcher?_pyMatcher->cuda():Parameters::defaultPyMatcherCuda();
167  std::string model = _pyMatcher?_pyMatcher->model():Parameters::defaultPyMatcherModel();
168  Parameters::parse(parameters, Parameters::kPyMatcherIterations(), iterations);
169  Parameters::parse(parameters, Parameters::kPyMatcherThreshold(), matchThr);
170  Parameters::parse(parameters, Parameters::kPyMatcherPath(), path);
171  Parameters::parse(parameters, Parameters::kPyMatcherCuda(), cuda);
172  Parameters::parse(parameters, Parameters::kPyMatcherModel(), model);
173  if(path.empty())
174  {
175  UERROR("%s parameter should be set to use Python3 matching (%s=6), using default %d.",
176  Parameters::kPyMatcherPath().c_str(),
177  Parameters::kVisCorNNType().c_str(),
178  Parameters::defaultVisCorNNType());
179  _nnType = Parameters::defaultVisCorNNType();
180  }
181  else
182  {
183  delete _pyMatcher;
184  _pyMatcher = new PyMatcher(path, matchThr, iterations, cuda, model);
185  }
186 #endif
187  }
188 #if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1))
189  else if(_nnType == 7)
190  {
191  UWARN("%s is set to 7 but RTAB-Map is not built with OpenCV's xfeatures2d support (OpenCV >= 3.4.1 also required), using default %d.",
192  Parameters::kVisCorNNType().c_str(), Parameters::defaultVisCorNNType());
193  _nnType = Parameters::defaultVisCorNNType();
194  }
195 #endif
196 
197  // override feature parameters
198  for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
199  {
200  std::string group = uSplit(iter->first, '/').front();
201  if(Parameters::isFeatureParameter(iter->first) || group.compare("Stereo") == 0)
202  {
203  uInsert(_featureParameters, ParametersPair(iter->first, iter->second));
204  }
205  }
206 
207  if(uContains(parameters, Parameters::kVisCorNNType()))
208  {
209  if(_nnType<VWDictionary::kNNUndef)
210  {
211  uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_nnType)));
212  }
213  }
214  if(uContains(parameters, Parameters::kVisCorNNDR()))
215  {
216  uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
217  }
218  if(uContains(parameters, Parameters::kKpByteToFloat()))
219  {
220  uInsert(_featureParameters, ParametersPair(Parameters::kKpByteToFloat(), parameters.at(Parameters::kKpByteToFloat())));
221  }
222  if(uContains(parameters, Parameters::kVisFeatureType()))
223  {
224  uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
225  }
226  if(uContains(parameters, Parameters::kVisMaxFeatures()))
227  {
228  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
229  }
230  if(uContains(parameters, Parameters::kVisMaxDepth()))
231  {
232  uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
233  }
234  if(uContains(parameters, Parameters::kVisMinDepth()))
235  {
236  uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
237  }
238  if(uContains(parameters, Parameters::kVisRoiRatios()))
239  {
240  uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
241  }
242  if(uContains(parameters, Parameters::kVisSubPixEps()))
243  {
244  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
245  }
246  if(uContains(parameters, Parameters::kVisSubPixIterations()))
247  {
248  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
249  }
250  if(uContains(parameters, Parameters::kVisSubPixWinSize()))
251  {
252  uInsert(_featureParameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
253  }
254  if(uContains(parameters, Parameters::kVisGridRows()))
255  {
256  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridRows(), parameters.at(Parameters::kVisGridRows())));
257  }
258  if(uContains(parameters, Parameters::kVisGridCols()))
259  {
260  uInsert(_featureParameters, ParametersPair(Parameters::kKpGridCols(), parameters.at(Parameters::kVisGridCols())));
261  }
262 
263  delete _detectorFrom;
264  delete _detectorTo;
265  _detectorFrom = Feature2D::create(_featureParameters);
266  _detectorTo = Feature2D::create(_featureParameters);
267 }
268 
269 RegistrationVis::~RegistrationVis()
270 {
271  delete _detectorFrom;
272  delete _detectorTo;
273 #ifdef RTABMAP_PYTHON
274  delete _pyMatcher;
275 #endif
276 }
277 
278 Transform RegistrationVis::computeTransformationImpl(
279  Signature & fromSignature,
280  Signature & toSignature,
281  Transform guess, // (flowMaxLevel is set to 0 when guess is used)
282  RegistrationInfo & info) const
283 {
284  UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
285  UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
286  UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
287  UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
288  UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
289  UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
290  UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
291  UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
292  UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
293  UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
294  UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
295  UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
296  UDEBUG("%s=%f", Parameters::kVisCorFlowEps().c_str(), _flowEps);
297  UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
298  UDEBUG("%s=%f", Parameters::kVisCorNNDR().c_str(), _nndr);
299  UDEBUG("%s=%d", Parameters::kVisCorNNType().c_str(), _nnType);
300  UDEBUG("%s=%d", Parameters::kVisCorGuessWinSize().c_str(), _guessWinSize);
301  UDEBUG("%s=%d", Parameters::kVisCorGuessMatchToProjection().c_str(), _guessMatchToProjection?1:0);
302  UDEBUG("Feature Detector = %d", (int)_detectorFrom->getType());
303  UDEBUG("guess=%s", guess.prettyPrint().c_str());
304 
305  UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
306  fromSignature.id(),
307  (int)fromSignature.getWords().size(),
308  (int)fromSignature.getWords3().size(),
309  (int)fromSignature.getWordsDescriptors().rows,
310  (int)fromSignature.sensorData().keypoints().size(),
311  (int)fromSignature.sensorData().keypoints3D().size(),
312  fromSignature.sensorData().descriptors().rows,
313  fromSignature.sensorData().imageRaw().cols,
314  fromSignature.sensorData().imageRaw().rows,
315  (int)fromSignature.sensorData().cameraModels().size(),
316  (int)fromSignature.sensorData().stereoCameraModels().size());
317 
318  UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
319  toSignature.id(),
320  (int)toSignature.getWords().size(),
321  (int)toSignature.getWords3().size(),
322  (int)toSignature.getWordsDescriptors().rows,
323  (int)toSignature.sensorData().keypoints().size(),
324  (int)toSignature.sensorData().keypoints3D().size(),
325  toSignature.sensorData().descriptors().rows,
326  toSignature.sensorData().imageRaw().cols,
327  toSignature.sensorData().imageRaw().rows,
328  (int)toSignature.sensorData().cameraModels().size(),
329  (int)toSignature.sensorData().stereoCameraModels().size());
330 
331  std::string msg;
332  info.projectedIDs.clear();
333 
335  // Find correspondences
337  //recompute correspondences if descriptors are provided
338  if((fromSignature.getWordsDescriptors().empty() && toSignature.getWordsDescriptors().empty()) &&
339  (_estimationType<2 || fromSignature.getWords().size()) && // required only for 2D->2D
340  (_estimationType==0 || toSignature.getWords().size()) && // required only for 3D->2D or 2D->2D
341  fromSignature.getWords3().size() && // required in all estimation approaches
342  (_estimationType==1 || toSignature.getWords3().size())) // required only for 3D->3D and 2D->2D
343  {
344  // no need to extract new features, we have all the data we need
345  UDEBUG("Bypassing feature matching as descriptors and images are empty. We assume features are already matched.");
346  }
347  else
348  {
349  UDEBUG("");
350  // just some checks to make sure that input data are ok
351  UASSERT(fromSignature.getWords().empty() ||
352  fromSignature.getWords3().empty() ||
353  (fromSignature.getWords().size() == fromSignature.getWords3().size()));
354  UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
355  (int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
356  fromSignature.sensorData().descriptors().empty() ||
357  fromSignature.getWordsDescriptors().empty() == 0);
358  UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
359  (toSignature.getWords().size() && toSignature.getWords3().empty())||
360  (toSignature.getWords().size() == toSignature.getWords3().size()));
361  UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
362  (int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
363  toSignature.sensorData().descriptors().empty() ||
364  toSignature.getWordsDescriptors().empty());
365  UASSERT(fromSignature.sensorData().imageRaw().empty() ||
366  fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
367  fromSignature.sensorData().imageRaw().type() == CV_8UC3);
368  UASSERT(toSignature.sensorData().imageRaw().empty() ||
369  toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
370  toSignature.sensorData().imageRaw().type() == CV_8UC3);
371 
372  std::vector<cv::KeyPoint> kptsFrom;
373  cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
374  cv::Mat imageTo = toSignature.sensorData().imageRaw();
375 
376  std::vector<int> orignalWordsFromIds;
377  int kptsFromSource = 0;
378  if(fromSignature.getWords().empty())
379  {
380  if(fromSignature.sensorData().keypoints().empty())
381  {
382  if(!imageFrom.empty())
383  {
384  if(imageFrom.channels() > 1)
385  {
386  cv::Mat tmp;
387  cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
388  imageFrom = tmp;
389  }
390 
391  cv::Mat depthMask;
392  if(!fromSignature.sensorData().depthRaw().empty() && _depthAsMask)
393  {
394  if(imageFrom.rows % fromSignature.sensorData().depthRaw().rows == 0 &&
395  imageFrom.cols % fromSignature.sensorData().depthRaw().cols == 0 &&
396  imageFrom.rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
397  {
398  depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
399  }
400  else
401  {
402  UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
403  Parameters::kVisDepthAsMask().c_str(),
404  fromSignature.sensorData().imageRaw().rows, fromSignature.sensorData().imageRaw().cols,
405  fromSignature.sensorData().depthRaw().rows, fromSignature.sensorData().depthRaw().cols);
406  }
407  }
408 
409  kptsFrom = _detectorFrom->generateKeypoints(
410  imageFrom,
411  depthMask);
412  }
413  }
414  else
415  {
416  kptsFrom = fromSignature.sensorData().keypoints();
417  kptsFromSource = 1;
418  }
419  }
420  else
421  {
422  kptsFromSource = 2;
423  orignalWordsFromIds.resize(fromSignature.getWords().size());
424  int i=0;
425  bool allUniques = true;
426  int previousIdAdded = 0;
427  kptsFrom = fromSignature.getWordsKpts();
428  for(std::multimap<int, int>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
429  {
430  UASSERT(iter->second>=0 && iter->second<(int)orignalWordsFromIds.size());
431  orignalWordsFromIds[iter->second] = iter->first;
432  if(i>0 && iter->first==previousIdAdded)
433  {
434  allUniques = false;
435  }
436  previousIdAdded = iter->first;
437  ++i;
438  }
439  if(!allUniques)
440  {
441  UDEBUG("IDs are not unique, IDs will be regenerated!");
442  orignalWordsFromIds.clear();
443  }
444  }
445 
446  std::multimap<int, int> wordsFrom;
447  std::multimap<int, int> wordsTo;
448  std::vector<cv::KeyPoint> wordsKptsFrom;
449  std::vector<cv::KeyPoint> wordsKptsTo;
450  std::vector<cv::Point3f> words3From;
451  std::vector<cv::Point3f> words3To;
452  cv::Mat wordsDescFrom;
453  cv::Mat wordsDescTo;
454  if(_correspondencesApproach == 1) //Optical Flow
455  {
456  UDEBUG("");
457  // convert to grayscale
458  if(imageFrom.channels() > 1)
459  {
460  cv::Mat tmp;
461  cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
462  imageFrom = tmp;
463  }
464  if(imageTo.channels() > 1)
465  {
466  cv::Mat tmp;
467  cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
468  imageTo = tmp;
469  }
470 
471  std::vector<cv::Point3f> kptsFrom3D;
472  if(kptsFrom.size() == fromSignature.getWords3().size())
473  {
474  kptsFrom3D = fromSignature.getWords3();
475  }
476  else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
477  {
478  kptsFrom3D = fromSignature.sensorData().keypoints3D();
479  }
480  else
481  {
482  kptsFrom3D = _detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
483  }
484 
485  if(!imageFrom.empty() && !imageTo.empty())
486  {
487  std::vector<cv::Point2f> cornersFrom;
488  cv::KeyPoint::convert(kptsFrom, cornersFrom);
489  std::vector<cv::Point2f> cornersTo;
490  bool guessSet = !guess.isIdentity() && !guess.isNull();
491  if(guessSet)
492  {
493  if(toSignature.sensorData().cameraModels().size() == 1 || toSignature.sensorData().stereoCameraModels().size() == 1)
494  {
495  Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModels()[0].left().localTransform();
496  Transform guessCameraRef = (guess * localTransform).inverse();
497  cv::Mat R = (cv::Mat_<double>(3,3) <<
498  (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
499  (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
500  (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
501  cv::Mat rvec(1,3, CV_64FC1);
502  cv::Rodrigues(R, rvec);
503  cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
504  cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModels()[0].left().K();
505  cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
506  }
507  else
508  {
509  UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored...");
510  }
511  }
512 
513  // Find features in the new left image
514  UDEBUG("guessSet = %d", guessSet?1:0);
515  std::vector<unsigned char> status;
516  std::vector<float> err;
517  UDEBUG("cv::calcOpticalFlowPyrLK() begin");
518  cv::calcOpticalFlowPyrLK(
519  imageFrom,
520  imageTo,
521  cornersFrom,
522  cornersTo,
523  status,
524  err,
525  cv::Size(_flowWinSize, _flowWinSize),
526  guessSet?0:_flowMaxLevel,
527  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
528  cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
529  UDEBUG("cv::calcOpticalFlowPyrLK() end");
530 
531  UASSERT(kptsFrom.size() == kptsFrom3D.size());
532  std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
533  std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
534  std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
535  int ki = 0;
536  for(unsigned int i=0; i<status.size(); ++i)
537  {
538  if(status[i] &&
539  uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
540  uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
541  {
542  if(orignalWordsFromIdsCpy.size())
543  {
544  orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[i];
545  }
546  kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
547  kptsFrom3DKept[ki] = kptsFrom3D[i];
548  kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
549  }
550  }
551  if(orignalWordsFromIds.size())
552  {
553  orignalWordsFromIds.resize(ki);
554  }
555  kptsFrom.resize(ki);
556  kptsTo.resize(ki);
557  kptsFrom3DKept.resize(ki);
558  kptsFrom3D = kptsFrom3DKept;
559 
560  std::vector<cv::Point3f> kptsTo3D;
561  if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
562  {
563  kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo);
564  }
565 
566  UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
567  UASSERT(kptsFrom.size() == kptsTo.size());
568  UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
569  for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
570  {
571  int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
572  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
573  wordsKptsFrom.push_back(kptsFrom[i]);
574  words3From.push_back(kptsFrom3DKept[i]);
575 
576  wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
577  wordsKptsTo.push_back(kptsTo[i]);
578  if(!kptsTo3D.empty())
579  {
580  words3To.push_back(kptsTo3D[i]);
581  }
582  }
583  toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
584  }
585  else
586  {
587  if(imageFrom.empty())
588  {
589  UERROR("Optical flow correspondences requires images in data!");
590  }
591  UASSERT(kptsFrom.size() == kptsFrom3D.size());
592  for(unsigned int i=0; i< kptsFrom3D.size(); ++i)
593  {
594  if(util3d::isFinite(kptsFrom3D[i]))
595  {
596  int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
597  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
598  wordsKptsFrom.push_back(kptsFrom[i]);
599  words3From.push_back(kptsFrom3D[i]);
600  }
601  }
602  toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
603  }
604 
605  fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, cv::Mat());
606  }
607  else // Features Matching
608  {
609  UDEBUG("");
610  std::vector<cv::KeyPoint> kptsTo;
611  int kptsToSource = 0;
612  if(toSignature.getWords().empty())
613  {
614  if(toSignature.sensorData().keypoints().empty() &&
615  !imageTo.empty())
616  {
617  if(imageTo.channels() > 1)
618  {
619  cv::Mat tmp;
620  cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
621  imageTo = tmp;
622  }
623 
624  cv::Mat depthMask;
625  if(!toSignature.sensorData().depthRaw().empty() && _depthAsMask)
626  {
627  if(imageTo.rows % toSignature.sensorData().depthRaw().rows == 0 &&
628  imageTo.cols % toSignature.sensorData().depthRaw().cols == 0 &&
629  imageTo.rows/toSignature.sensorData().depthRaw().rows == imageTo.cols/toSignature.sensorData().depthRaw().cols)
630  {
631  depthMask = util2d::interpolate(toSignature.sensorData().depthRaw(), imageTo.rows/toSignature.sensorData().depthRaw().rows, 0.1f);
632  }
633  else
634  {
635  UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
636  Parameters::kVisDepthAsMask().c_str(),
637  toSignature.sensorData().imageRaw().rows, toSignature.sensorData().imageRaw().cols,
638  toSignature.sensorData().depthRaw().rows, toSignature.sensorData().depthRaw().cols);
639  }
640  }
641 
642  kptsTo = _detectorTo->generateKeypoints(
643  imageTo,
644  depthMask);
645  }
646  else
647  {
648  kptsTo = toSignature.sensorData().keypoints();
649  kptsToSource = 1;
650  }
651  }
652  else
653  {
654  kptsTo = toSignature.getWordsKpts();
655  kptsToSource = 2;
656  }
657 
658  // extract descriptors
659  UDEBUG("kptsFrom=%d kptsFromSource=%d", (int)kptsFrom.size(), kptsFromSource);
660  UDEBUG("kptsTo=%d kptsToSource=%d", (int)kptsTo.size(), kptsToSource);
661  cv::Mat descriptorsFrom;
662  if(kptsFromSource == 2 &&
663  fromSignature.getWordsDescriptors().rows &&
664  ((kptsFrom.empty() && fromSignature.getWordsDescriptors().rows) ||
665  fromSignature.getWordsDescriptors().rows == (int)kptsFrom.size()))
666  {
667  descriptorsFrom = fromSignature.getWordsDescriptors();
668  }
669  else if(kptsFromSource == 1 &&
670  fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
671  {
672  descriptorsFrom = fromSignature.sensorData().descriptors();
673  }
674  else if(!imageFrom.empty())
675  {
676  if(imageFrom.channels() > 1)
677  {
678  cv::Mat tmp;
679  cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
680  imageFrom = tmp;
681  }
682  UDEBUG("cleared orignalWordsFromIds");
683  orignalWordsFromIds.clear();
684  descriptorsFrom = _detectorFrom->generateDescriptors(imageFrom, kptsFrom);
685  }
686 
687  cv::Mat descriptorsTo;
688  if(kptsTo.size())
689  {
690  if(kptsToSource == 2 &&
691  toSignature.getWordsDescriptors().rows == (int)kptsTo.size())
692  {
693  descriptorsTo = toSignature.getWordsDescriptors();
694  }
695  else if(kptsToSource == 1 &&
696  toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
697  {
698  descriptorsTo = toSignature.sensorData().descriptors();
699  }
700  else if(!imageTo.empty())
701  {
702  if(imageTo.channels() > 1)
703  {
704  cv::Mat tmp;
705  cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
706  imageTo = tmp;
707  }
708 
709  descriptorsTo = _detectorTo->generateDescriptors(imageTo, kptsTo);
710  }
711  }
712 
713  // create 3D keypoints
714  std::vector<cv::Point3f> kptsFrom3D;
715  std::vector<cv::Point3f> kptsTo3D;
716  if(kptsFromSource == 2 &&
717  kptsFrom.size() == fromSignature.getWords3().size())
718  {
719  kptsFrom3D = fromSignature.getWords3();
720  }
721  else if(kptsFromSource == 1 &&
722  kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
723  {
724  kptsFrom3D = fromSignature.sensorData().keypoints3D();
725  }
726  else
727  {
728  if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
729  {
730  UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
731  "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
732  kptsFrom.size(),
733  fromSignature.getWords3().size());
734  }
735  else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
736  {
737  UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
738  "is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
739  kptsFrom.size(),
740  fromSignature.sensorData().keypoints3D().size());
741  }
742  kptsFrom3D = _detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
743  UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
744  }
745 
746  if(!kptsFrom3D.empty() &&
747  (_detectorFrom->getMinDepth() > 0.0f || _detectorFrom->getMaxDepth() > 0.0f) &&
748  (!fromSignature.sensorData().cameraModels().empty() || !fromSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
749  {
750  _detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
751  }
752 
753  if(kptsToSource == 2 && kptsTo.size() == toSignature.getWords3().size())
754  {
755  kptsTo3D = toSignature.getWords3();
756  }
757  else if(kptsToSource == 1 &&
758  kptsTo.size() == toSignature.sensorData().keypoints3D().size())
759  {
760  kptsTo3D = toSignature.sensorData().keypoints3D();
761  }
762  else
763  {
764  if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
765  {
766  UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
767  "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
768  (int)kptsTo.size(),
769  (int)toSignature.getWords3().size());
770  }
771  else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
772  {
773  UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
774  "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
775  (int)kptsTo.size(),
776  (int)toSignature.sensorData().keypoints3D().size());
777  }
778  kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo);
779  }
780 
781  if(kptsTo3D.size() &&
782  (_detectorTo->getMinDepth() > 0.0f || _detectorTo->getMaxDepth() > 0.0f) &&
783  (!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
784  {
785  _detectorTo->filterKeypointsByDepth(kptsTo, descriptorsTo, kptsTo3D, _detectorTo->getMinDepth(), _detectorTo->getMaxDepth());
786  }
787 
788  UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);
789 
790  fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, descriptorsFrom);
791  toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, descriptorsTo);
792 
793  UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
794  UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
795  UDEBUG("orignalWordsFromIds=%d", (int)orignalWordsFromIds.size());
796 
797  // We have all data we need here, so match!
798  if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
799  {
800  std::vector<CameraModel> models;
801  if(!toSignature.sensorData().stereoCameraModels().empty())
802  {
803  for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
804  {
805  models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
806  }
807  }
808  else
809  {
810  models = toSignature.sensorData().cameraModels();
811  }
812 
813  bool isCalibrated = !models.empty();
814  for(size_t i=0; i<models.size() && isCalibrated; ++i)
815  {
816  isCalibrated = models[i].isValidForProjection();
817 
818  // For old database formats
819  if(isCalibrated && (models[i].imageWidth()==0 || models[i].imageHeight()==0))
820  {
821  if(!toSignature.sensorData().imageRaw().empty())
822  {
823  models[i].setImageSize(cv::Size(toSignature.sensorData().imageRaw().cols/models.size(), toSignature.sensorData().imageRaw().rows));
824  }
825  else
826  {
827  isCalibrated = false;
828  }
829  }
830  }
831 
832  // If guess is set, limit the search of matches using optical flow window size
833  bool guessSet = !guess.isIdentity() && !guess.isNull();
834  if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() &&
835  isCalibrated && // needed for projection
836  _estimationType != 2) // To make sure we match all features for 2D->2D
837  {
838  // Use guess to project 3D "from" keypoints into "to" image
839  UDEBUG("");
840  UASSERT((int)kptsTo.size() == descriptorsTo.rows);
841  UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
842 
843  std::vector<cv::Point2f> cornersProjected;
844  std::vector<int> projectedIndexToDescIndex;
845  float subImageWidth = models[0].imageWidth();
846  std::set<int> added;
847  int duplicates=0;
848  for(size_t m=0; m<models.size(); ++m)
849  {
850  Transform guessCameraRef = (guess * models[m].localTransform()).inverse();
851  cv::Mat R = (cv::Mat_<double>(3,3) <<
852  (double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
853  (double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
854  (double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
855  cv::Mat rvec(1,3, CV_64FC1);
856  cv::Rodrigues(R, rvec);
857  cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
858  cv::Mat K = models[m].K();
859  std::vector<cv::Point2f> projected;
860  cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
861  UDEBUG("Projected points=%d", (int)projected.size());
862 
863  //remove projected points outside of the image
864  UASSERT((int)projected.size() == descriptorsFrom.rows);
865  int cornersInFrame = 0;
866  for(unsigned int i=0; i<projected.size(); ++i)
867  {
868  if(uIsInBounds(projected[i].x, 0.0f, float(models[m].imageWidth()-1)) &&
869  uIsInBounds(projected[i].y, 0.0f, float(models[m].imageHeight()-1)) &&
870  util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
871  {
872  if(added.find(i) != added.end())
873  {
874  ++duplicates;
875  continue;
876  }
877 
878  projectedIndexToDescIndex.push_back(i);
879  projected[i].x += subImageWidth*float(m); // Convert in multicam stitched image
880  cornersProjected.push_back(projected[i]);
881  ++cornersInFrame;
882  added.insert(i);
883  }
884  }
885  UDEBUG("corners in frame=%d (camera index=%ld)", cornersInFrame, m);
886  }
887 
888  // For each projected feature guess of "from" in "to", find its matching feature in
889  // the radius around the projected guess.
890  // TODO: do cross-check?
891  UDEBUG("guessMatchToProjection=%d, cornersProjected=%d orignalWordsFromIds=%d (added=%ld, duplicates=%d)",
892  _guessMatchToProjection?1:0, (int)cornersProjected.size(), (int)orignalWordsFromIds.size(),
893  added.size(), duplicates);
894  if(cornersProjected.size())
895  {
896  if(_guessMatchToProjection)
897  {
898  UDEBUG("match frame to projected");
899  // Create kd-tree for projected keypoints
900  rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
901  rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
902  index.buildIndex();
903 
904  std::vector< std::vector<size_t> > indices;
905  std::vector<std::vector<float> > dists;
906  float radius = (float)_guessWinSize; // pixels
907  std::vector<cv::Point2f> pointsTo;
908  cv::KeyPoint::convert(kptsTo, pointsTo);
909  rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
910  index.radiusSearch(pointsToMat, indices, dists, radius*radius, rtflann::SearchParams());
911 
912  UASSERT(indices.size() == pointsToMat.rows);
913  UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
914  UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
915  UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
916  UASSERT(pointsToMat.rows == kptsTo.size());
917  UDEBUG("radius search done for guess");
918 
919  // Process results (Nearest Neighbor Distance Ratio)
920  int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
921  std::map<int,int> addedWordsFrom; //<id, index>
922  std::map<int, int> duplicates; //<fromId, toId>
923  int newWords = 0;
924  cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
925  for(unsigned int i = 0; i < pointsToMat.rows; ++i)
926  {
927  int matchedIndex = -1;
928  if(indices[i].size() >= 2)
929  {
930  std::vector<int> descriptorsIndices(indices[i].size());
931  int oi=0;
932  if((int)indices[i].size() > descriptors.rows)
933  {
934  descriptors.resize(indices[i].size());
935  }
936  for(unsigned int j=0; j<indices[i].size(); ++j)
937  {
938  descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
939  descriptorsIndices[oi++] = indices[i].at(j);
940  }
941  descriptorsIndices.resize(oi);
942  UASSERT(oi >=2);
943 
944  cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _nnType == 5);
945  if(_nnType == 5) // bruteforce cross check
946  {
947  std::vector<cv::DMatch> matches;
948  matcher.match(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches);
949  if(!matches.empty())
950  {
951  matchedIndex = descriptorsIndices.at(matches.at(0).trainIdx);
952  }
953  }
954  else // bruteforce knn
955  {
956  std::vector<std::vector<cv::DMatch> > matches;
957  matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
958  UASSERT(matches.size() == 1);
959  UASSERT(matches[0].size() == 2);
960  if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
961  {
962  matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
963  }
964  }
965  }
966  else if(indices[i].size() == 1)
967  {
968  matchedIndex = indices[i].at(0);
969  }
970 
971  if(matchedIndex >= 0)
972  {
973  matchedIndex = projectedIndexToDescIndex[matchedIndex];
974  int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndex]:matchedIndex;
975 
976  if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
977  {
978  id = addedWordsFrom.at(matchedIndex);
979  duplicates.insert(std::make_pair(matchedIndex, id));
980  }
981  else
982  {
983  addedWordsFrom.insert(std::make_pair(matchedIndex, id));
984 
985  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
986  if(!kptsFrom.empty())
987  {
988  wordsKptsFrom.push_back(kptsFrom[matchedIndex]);
989  }
990  words3From.push_back(kptsFrom3D[matchedIndex]);
991  wordsDescFrom.push_back(descriptorsFrom.row(matchedIndex));
992  }
993 
994  wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
995  wordsKptsTo.push_back(kptsTo[i]);
996  wordsDescTo.push_back(descriptorsTo.row(i));
997  if(!kptsTo3D.empty())
998  {
999  words3To.push_back(kptsTo3D[i]);
1000  }
1001  }
1002  else
1003  {
1004  // gen fake ids
1005  wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
1006  wordsKptsTo.push_back(kptsTo[i]);
1007  wordsDescTo.push_back(descriptorsTo.row(i));
1008  if(!kptsTo3D.empty())
1009  {
1010  words3To.push_back(kptsTo3D[i]);
1011  }
1012 
1013  ++newToId;
1014  ++newWords;
1015  }
1016  }
1017  UDEBUG("addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
1018  (int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
1019  (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
1020 
1021  // create fake ids for not matched words from "from"
1022  int addWordsFromNotMatched = 0;
1023  for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
1024  {
1025  if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
1026  {
1027  int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
1028  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
1029  wordsKptsFrom.push_back(kptsFrom[i]);
1030  wordsDescFrom.push_back(descriptorsFrom.row(i));
1031  words3From.push_back(kptsFrom3D[i]);
1032 
1033  ++addWordsFromNotMatched;
1034  }
1035  }
1036  UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
1037  }
1038  else
1039  {
1040  UDEBUG("match projected to frame");
1041  std::vector<cv::Point2f> pointsTo;
1042  cv::KeyPoint::convert(kptsTo, pointsTo);
1043  rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
1044  rtflann::Index<rtflann::L2_Simple<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
1045  index.buildIndex();
1046 
1047  std::vector< std::vector<size_t> > indices;
1048  std::vector<std::vector<float> > dists;
1049  float radius = (float)_guessWinSize; // pixels
1050  rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
1051  index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
1052 
1053  UASSERT(indices.size() == cornersProjectedMat.rows);
1054  UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
1055  UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
1056  UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
1057  UASSERT(pointsToMat.rows == kptsTo.size());
1058  UDEBUG("radius search done for guess");
1059 
1060  // Process results (Nearest Neighbor Distance Ratio)
1061  std::set<int> addedWordsTo;
1062  std::set<int> addedWordsFrom;
1063  double bruteForceTotalTime = 0.0;
1064  double bruteForceDescCopy = 0.0;
1065  UTimer bruteForceTimer;
1066  cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
1067  for(unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
1068  {
1069  int matchedIndexFrom = projectedIndexToDescIndex[i];
1070 
1071  if(indices[i].size())
1072  {
1073  info.projectedIDs.push_back(!orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
1074  }
1075 
1076  if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
1077  {
1078  int matchedIndexTo = -1;
1079  if(indices[i].size() >= 2)
1080  {
1081  bruteForceTimer.restart();
1082  std::vector<int> descriptorsIndices(indices[i].size());
1083  int oi=0;
1084  if((int)indices[i].size() > descriptors.rows)
1085  {
1086  descriptors.resize(indices[i].size());
1087  }
1088  for(unsigned int j=0; j<indices[i].size(); ++j)
1089  {
1090  descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
1091  descriptorsIndices[oi++] = indices[i].at(j);
1092  }
1093  bruteForceDescCopy += bruteForceTimer.ticks();
1094  UASSERT(oi >=2);
1095 
1096  cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, _nnType==5);
1097  if(_nnType==5) // bruteforce cross check
1098  {
1099  std::vector<cv::DMatch> matches;
1100  matcher.match(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches);
1101  if(!matches.empty())
1102  {
1103  matchedIndexTo = descriptorsIndices.at(matches.at(0).trainIdx);
1104  }
1105  }
1106  else // bruteforce knn
1107  {
1108  std::vector<std::vector<cv::DMatch> > matches;
1109  matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
1110  UASSERT(matches.size() == 1);
1111  UASSERT(matches[0].size() == 2);
1112  bruteForceTotalTime+=bruteForceTimer.elapsed();
1113  if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
1114  {
1115  matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
1116  }
1117  }
1118  }
1119  else if(indices[i].size() == 1)
1120  {
1121  matchedIndexTo = indices[i].at(0);
1122  }
1123 
1124  int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
1125  addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
1126 
1127  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
1128  if(!kptsFrom.empty())
1129  {
1130  wordsKptsFrom.push_back(kptsFrom[matchedIndexFrom]);
1131  }
1132  words3From.push_back(kptsFrom3D[matchedIndexFrom]);
1133  wordsDescFrom.push_back(descriptorsFrom.row(matchedIndexFrom));
1134 
1135  if( matchedIndexTo >= 0 &&
1136  addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
1137  {
1138  addedWordsTo.insert(matchedIndexTo);
1139 
1140  wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
1141  wordsKptsTo.push_back(kptsTo[matchedIndexTo]);
1142  wordsDescTo.push_back(descriptorsTo.row(matchedIndexTo));
1143  if(!kptsTo3D.empty())
1144  {
1145  words3To.push_back(kptsTo3D[matchedIndexTo]);
1146  }
1147  }
1148  }
1149  }
1150  UDEBUG("bruteForceDescCopy=%fs, bruteForceTotalTime=%fs", bruteForceDescCopy, bruteForceTotalTime);
1151 
1152  // create fake ids for not matched words from "from"
1153  for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
1154  {
1155  if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
1156  {
1157  int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
1158  wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
1159  wordsKptsFrom.push_back(kptsFrom[i]);
1160  wordsDescFrom.push_back(descriptorsFrom.row(i));
1161  words3From.push_back(kptsFrom3D[i]);
1162  }
1163  }
1164 
1165  int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
1166  for(unsigned int i = 0; i < kptsTo.size(); ++i)
1167  {
1168  if(addedWordsTo.find(i) == addedWordsTo.end())
1169  {
1170  wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
1171  wordsKptsTo.push_back(kptsTo[i]);
1172  wordsDescTo.push_back(descriptorsTo.row(i));
1173  if(!kptsTo3D.empty())
1174  {
1175  words3To.push_back(kptsTo3D[i]);
1176  }
1177  ++newToId;
1178  }
1179  }
1180  }
1181  }
1182  else
1183  {
1184  UWARN("All projected points are outside the camera. Guess (%s) is wrong or images are not overlapping.", guess.prettyPrint().c_str());
1185  }
1186  UDEBUG("");
1187  }
1188  else
1189  {
1190  if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
1191  {
1192  UWARN("Calibration not found! Finding correspondences "
1193  "with the guess cannot be done, global matching is "
1194  "done instead.");
1195  }
1196 
1197  UDEBUG("");
1198  // match between all descriptors
1199  std::list<int> fromWordIds;
1200  std::list<int> toWordIds;
1201 #ifdef RTABMAP_PYTHON
1202  if(_nnType == 5 || (_nnType == 6 && _pyMatcher) || _nnType==7)
1203 #else
1204  if(_nnType == 5 || _nnType == 7) // bruteforce cross check or GMS
1205 #endif
1206  {
1207  std::vector<int> fromWordIdsV(descriptorsFrom.rows);
1208  for (int i = 0; i < descriptorsFrom.rows; ++i)
1209  {
1210  int id = i+1;
1211  if(!orignalWordsFromIds.empty())
1212  {
1213  id = orignalWordsFromIds[i];
1214  }
1215  fromWordIds.push_back(id);
1216  fromWordIdsV[i] = id;
1217  }
1218  if(descriptorsTo.rows)
1219  {
1220  std::vector<int> toWordIdsV(descriptorsTo.rows, 0);
1221  std::vector<cv::DMatch> matches;
1222 #ifdef RTABMAP_PYTHON
1223  if(_nnType == 6 && _pyMatcher &&
1224  descriptorsTo.cols == descriptorsFrom.cols &&
1225  descriptorsTo.rows == (int)kptsTo.size() &&
1226  descriptorsTo.type() == CV_32F &&
1227  descriptorsFrom.type() == CV_32F &&
1228  descriptorsFrom.rows == (int)kptsFrom.size() &&
1229  models.size() == 1)
1230  {
1231  UDEBUG("Python matching");
1232  matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, models[0].imageSize());
1233  }
1234  else
1235  {
1236  if(_nnType == 6 && _pyMatcher)
1237  {
1238  UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported, multicam not supported), doing bruteforce matching instead.", descriptorsFrom.type());
1239  }
1240 #else
1241  {
1242 #endif
1243  bool doCrossCheck = true;
1244 #ifdef HAVE_OPENCV_XFEATURES2D
1245 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1)
1246  cv::Size imageSizeFrom;
1247  if(_nnType == 7)
1248  {
1249  imageSizeFrom = imageFrom.size();
1250  if((imageSizeFrom.height == 0 || imageSizeFrom.width == 0) && (fromSignature.sensorData().cameraModels().size() || fromSignature.sensorData().stereoCameraModels().size()))
1251  {
1252  imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModels()[0].left().imageSize();
1253  }
1254  if(!models.empty() && models[0].imageSize().height > 0 && models[0].imageSize().width > 0 &&
1255  imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
1256  {
1257  doCrossCheck = false;
1258  }
1259  else
1260  {
1261  UDEBUG("Invalid inputs for GMS matching, image size should be set for both inputs, doing bruteforce matching instead.");
1262  }
1263  }
1264 #endif
1265 #endif
1266 
1267  UDEBUG("BruteForce matching%s", _nnType!=7?" with crosscheck":" with GMS");
1268  cv::BFMatcher matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, doCrossCheck);
1269  matcher.match(descriptorsTo, descriptorsFrom, matches);
1270 
1271 #if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
1272  if(!doCrossCheck)
1273  {
1274  UASSERT(!models.empty());
1275  std::vector<cv::DMatch> matchesGMS;
1276  cv::xfeatures2d::matchGMS(models[0].imageSize(), imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
1277  matches = matchesGMS;
1278  }
1279 #endif
1280  }
1281  for(size_t i=0; i<matches.size(); ++i)
1282  {
1283  toWordIdsV[matches[i].queryIdx] = fromWordIdsV[matches[i].trainIdx];
1284  }
1285  for(size_t i=0; i<toWordIdsV.size(); ++i)
1286  {
1287  int toId = toWordIdsV[i];
1288  if(toId==0)
1289  {
1290  toId = fromWordIds.back()+i+1;
1291  }
1292  toWordIds.push_back(toId);
1293  }
1294  }
1295  }
1296  else
1297  {
1298  UDEBUG("VWDictionary knn matching");
1299  VWDictionary dictionary(_featureParameters);
1300  if(orignalWordsFromIds.empty())
1301  {
1302  fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
1303  }
1304  else
1305  {
1306  for (int i = 0; i < descriptorsFrom.rows; ++i)
1307  {
1308  int id = orignalWordsFromIds[i];
1309  dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
1310  fromWordIds.push_back(id);
1311  }
1312  }
1313 
1314  if(descriptorsTo.rows)
1315  {
1316  dictionary.update();
1317  toWordIds = dictionary.addNewWords(descriptorsTo, 2);
1318  }
1319  dictionary.clear(false);
1320  }
1321 
1322  std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
1323  std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
1324 
1325  UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
1326  UASSERT(int(fromWordIds.size()) == descriptorsFrom.rows);
1327  int i=0;
1328  for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
1329  {
1330  if(fromWordIdsSet.count(*iter) == 1)
1331  {
1332  wordsFrom.insert(wordsFrom.end(), std::make_pair(*iter, wordsFrom.size()));
1333  if (!kptsFrom.empty())
1334  {
1335  wordsKptsFrom.push_back(kptsFrom[i]);
1336  }
1337  if(!kptsFrom3D.empty())
1338  {
1339  words3From.push_back(kptsFrom3D[i]);
1340  }
1341  wordsDescFrom.push_back(descriptorsFrom.row(i));
1342  }
1343  ++i;
1344  }
1345  UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
1346  UASSERT(toWordIds.size() == kptsTo.size());
1347  UASSERT(int(toWordIds.size()) == descriptorsTo.rows);
1348  i=0;
1349  for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
1350  {
1351  if(toWordIdsSet.count(*iter) == 1)
1352  {
1353  wordsTo.insert(wordsTo.end(), std::make_pair(*iter, wordsTo.size()));
1354  wordsKptsTo.push_back(kptsTo[i]);
1355  wordsDescTo.push_back(descriptorsTo.row(i));
1356  if(!kptsTo3D.empty())
1357  {
1358  words3To.push_back(kptsTo3D[i]);
1359  }
1360  }
1361  ++i;
1362  }
1363  }
1364  }
1365  else if(descriptorsFrom.rows)
1366  {
1367  //just create fake words
1368  UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
1369  for(int i=0; i<descriptorsFrom.rows; ++i)
1370  {
1371  wordsFrom.insert(wordsFrom.end(), std::make_pair(i, wordsFrom.size()));
1372  wordsKptsFrom.push_back(kptsFrom[i]);
1373  wordsDescFrom.push_back(descriptorsFrom.row(i));
1374  if(!kptsFrom3D.empty())
1375  {
1376  words3From.push_back(kptsFrom3D[i]);
1377  }
1378  }
1379  }
1380  }
1381 
1382  fromSignature.setWords(wordsFrom, wordsKptsFrom, words3From, wordsDescFrom);
1383  toSignature.setWords(wordsTo, wordsKptsTo, words3To, wordsDescTo);
1384  }
1385 
1387  // Motion estimation
1389  Transform transform;
1390  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1391  int inliersCount = 0;
1392  int matchesCount = 0;
1393  info.inliersIDs.clear();
1394  info.matchesIDs.clear();
1395  if(toSignature.getWords().size())
1396  {
1397  Transform transforms[2];
1398  std::vector<int> inliers[2];
1399  std::vector<int> matches[2];
1400  cv::Mat covariances[2];
1401  covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
1402  covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
1403  for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
1404  {
1405  // A to B
1406  Signature * signatureA;
1407  Signature * signatureB;
1408  if(dir == 0)
1409  {
1410  signatureA = &fromSignature;
1411  signatureB = &toSignature;
1412  }
1413  else
1414  {
1415  signatureA = &toSignature;
1416  signatureB = &fromSignature;
1417  }
1418  if(_estimationType == 2) // Epipolar Geometry
1419  {
1420  UDEBUG("");
1421  if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
1422  !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
1423  (signatureB->sensorData().cameraModels().size() != 1 ||
1424  !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
1425  {
1426  UERROR("Calibrated camera required (multi-cameras not supported).");
1427  }
1428  else if((int)signatureA->getWords().size() >= _minInliers &&
1429  (int)signatureB->getWords().size() >= _minInliers)
1430  {
1431  UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
1432  const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
1433 
1434  // we only need the camera transform, send guess words3 for scale estimation
1435  Transform cameraTransform;
1436  double variance = 1.0f;
1437  std::vector<int> matchesV;
1438  std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
1439  std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
1440  std::map<int, cv::KeyPoint> wordsA;
1441  std::map<int, cv::Point3f> words3A;
1442  std::map<int, cv::KeyPoint> wordsB;
1443  for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
1444  {
1445  wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
1446  if(!signatureA->getWords3().empty())
1447  {
1448  words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
1449  }
1450  }
1451  for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
1452  {
1453  wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
1454  }
1455  std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
1456  wordsA,
1457  wordsB,
1458  cameraModel,
1459  cameraTransform,
1460  _PnPReprojError,
1461  0.99f,
1462  words3A, // for scale estimation
1463  &variance,
1464  &matchesV);
1465  covariances[dir] *= variance;
1466  inliers[dir] = uKeys(inliers3D);
1467  matches[dir] = matchesV;
1468 
1469  if(!cameraTransform.isNull())
1470  {
1471  if((int)inliers3D.size() >= _minInliers)
1472  {
1473  if(variance <= _epipolarGeometryVar)
1474  {
1475  if(this->force3DoF())
1476  {
1477  transforms[dir] = cameraTransform.to3DoF();
1478  }
1479  else
1480  {
1481  transforms[dir] = cameraTransform;
1482  }
1483  }
1484  else
1485  {
1486  msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
1487  UINFO(msg.c_str());
1488  }
1489  }
1490  else
1491  {
1492  msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
1493  UINFO(msg.c_str());
1494  }
1495  }
1496  else
1497  {
1498  msg = uFormat("No camera transform found");
1499  UINFO(msg.c_str());
1500  }
1501  }
1502  else if(signatureA->getWords().size() == 0)
1503  {
1504  msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
1505  UWARN(msg.c_str());
1506  }
1507  else
1508  {
1509  msg = uFormat("No camera model");
1510  UWARN(msg.c_str());
1511  }
1512  }
1513  else if(_estimationType == 1) // PnP
1514  {
1515  UDEBUG("");
1516  if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
1517  (signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
1518  {
1519  UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
1520  signatureB->id(),
1521  (int)signatureB->sensorData().cameraModels().size(),
1522  signatureB->sensorData().stereoCameraModels().size(),
1523  signatureB->getWeight());
1524  }
1525 #ifndef RTABMAP_OPENGV
1526  else if(signatureB->sensorData().cameraModels().size() > 1)
1527  {
1528  UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
1529  "with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
1530  }
1531 #endif
1532  else
1533  {
1534  UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
1535  // 3D to 2D
1536  if((int)signatureA->getWords3().size() >= _minInliers &&
1537  (int)signatureB->getWords().size() >= _minInliers)
1538  {
1539  std::vector<int> inliersV;
1540  std::vector<int> matchesV;
1541  std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
1542  std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
1543  std::map<int, cv::Point3f> words3A;
1544  std::map<int, cv::Point3f> words3B;
1545  std::map<int, cv::KeyPoint> wordsB;
1546  for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
1547  {
1548  words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
1549  }
1550  for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
1551  {
1552  wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
1553  if(!signatureB->getWords3().empty())
1554  {
1555  words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
1556  }
1557  }
1558 
1559  std::vector<CameraModel> models;
1560  if(signatureB->sensorData().stereoCameraModels().size())
1561  {
1562  for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
1563  {
1564  models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
1565  }
1566  }
1567  else
1568  {
1569  models = signatureB->sensorData().cameraModels();
1570  }
1571 
1572  if(models.size()>1)
1573  {
1574  // Multi-Camera
1575  UASSERT(models[0].isValidForProjection());
1576 
1577  transforms[dir] = util3d::estimateMotion3DTo2D(
1578  words3A,
1579  wordsB,
1580  models,
1581  _minInliers,
1582  _iterations,
1583  _PnPReprojError,
1584  _PnPFlags,
1585  _PnPRefineIterations,
1586  _PnPMaxVar,
1587  dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
1588  words3B,
1589  &covariances[dir],
1590  &matchesV,
1591  &inliersV);
1592  inliers[dir] = inliersV;
1593  matches[dir] = matchesV;
1594  }
1595  else
1596  {
1597  UASSERT(models.size() == 1 && models[0].isValidForProjection());
1598 
1599  transforms[dir] = util3d::estimateMotion3DTo2D(
1600  words3A,
1601  wordsB,
1602  models[0],
1603  _minInliers,
1604  _iterations,
1605  _PnPReprojError,
1606  _PnPFlags,
1607  _PnPRefineIterations,
1608  _PnPMaxVar,
1609  dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
1610  words3B,
1611  &covariances[dir],
1612  &matchesV,
1613  &inliersV);
1614  inliers[dir] = inliersV;
1615  matches[dir] = matchesV;
1616  }
1617  UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
1618  if(transforms[dir].isNull())
1619  {
1620  msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
1621  (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
1622  UINFO(msg.c_str());
1623  }
1624  else if(this->force3DoF())
1625  {
1626  transforms[dir] = transforms[dir].to3DoF();
1627  }
1628  }
1629  else
1630  {
1631  msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
1632  (int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
1633  UINFO(msg.c_str());
1634  }
1635  }
1636 
1637  }
1638  else
1639  {
1640  UDEBUG("");
1641  // 3D -> 3D
1642  if((int)signatureA->getWords3().size() >= _minInliers &&
1643  (int)signatureB->getWords3().size() >= _minInliers)
1644  {
1645  std::vector<int> inliersV;
1646  std::vector<int> matchesV;
1647  std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
1648  std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
1649  std::map<int, cv::Point3f> words3A;
1650  std::map<int, cv::Point3f> words3B;
1651  for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
1652  {
1653  words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
1654  }
1655  for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
1656  {
1657  words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
1658  }
1659  transforms[dir] = util3d::estimateMotion3DTo3D(
1660  words3A,
1661  words3B,
1662  _minInliers,
1663  _inlierDistance,
1664  _iterations,
1665  _refineIterations,
1666  &covariances[dir],
1667  &matchesV,
1668  &inliersV);
1669  inliers[dir] = inliersV;
1670  matches[dir] = matchesV;
1671  UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
1672  if(transforms[dir].isNull())
1673  {
1674  msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
1675  (int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
1676  UINFO(msg.c_str());
1677  }
1678  else if(this->force3DoF())
1679  {
1680  transforms[dir] = transforms[dir].to3DoF();
1681  }
1682  }
1683  else
1684  {
1685  msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
1686  (int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
1687  UINFO(msg.c_str());
1688  }
1689  }
1690  }
1691 
1692  if(!_forwardEstimateOnly)
1693  {
1694  UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
1695  UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
1696  }
1697 
1698  std::vector<int> allInliers = inliers[0];
1699  if(inliers[1].size())
1700  {
1701  std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
1702  unsigned int oi = allInliers.size();
1703  allInliers.resize(allInliers.size() + inliers[1].size());
1704  for(unsigned int i=0; i<inliers[1].size(); ++i)
1705  {
1706  if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
1707  {
1708  allInliers[oi++] = inliers[1][i];
1709  }
1710  }
1711  allInliers.resize(oi);
1712  }
1713  std::vector<int> allMatches = matches[0];
1714  if(matches[1].size())
1715  {
1716  std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
1717  unsigned int oi = allMatches.size();
1718  allMatches.resize(allMatches.size() + matches[1].size());
1719  for(unsigned int i=0; i<matches[1].size(); ++i)
1720  {
1721  if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
1722  {
1723  allMatches[oi++] = matches[1][i];
1724  }
1725  }
1726  allMatches.resize(oi);
1727  }
1728 
1729  if(_bundleAdjustment > 0 &&
1730  _estimationType < 2 &&
1731  !transforms[0].isNull() &&
1732  allInliers.size() &&
1733  fromSignature.getWords3().size() &&
1734  toSignature.getWords().size() &&
1735  (fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
1736  (toSignature.sensorData().stereoCameraModels().size() >= 1 || toSignature.sensorData().cameraModels().size() >= 1))
1737  {
1738  UDEBUG("Refine with bundle adjustment");
1739  Optimizer * sba = Optimizer::create(_bundleAdjustment==3?Optimizer::kTypeCeres:_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
1740 
1741  std::map<int, Transform> poses;
1742  std::multimap<int, Link> links;
1743  std::map<int, cv::Point3f> points3DMap;
1744 
1745  poses.insert(std::make_pair(1, Transform::getIdentity()));
1746  poses.insert(std::make_pair(2, transforms[0]));
1747 
1748  for(int i=0;i<2;++i)
1749  {
1750  UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
1751  if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
1752  covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
1753  if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
1754  covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
1755  if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
1756  covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
1757  if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
1758  covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
1759  if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
1760  covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
1761  if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
1762  covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
1763  }
1764 
1765  cv::Mat cov = covariances[0].clone();
1766 
1767  links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
1768  if(!transforms[1].isNull() && inliers[1].size())
1769  {
1770  cov = covariances[1].clone();
1771  links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
1772  }
1773 
1774  std::map<int, Transform> optimizedPoses;
1775 
1776  UASSERT((toSignature.sensorData().stereoCameraModels().size() >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
1777  (toSignature.sensorData().cameraModels().size() >= 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
1778 
1779  std::map<int, std::vector<CameraModel> > models;
1780 
1781  std::vector<CameraModel> cameraModelsFrom;
1782  if(fromSignature.sensorData().stereoCameraModels().size())
1783  {
1784  for(size_t i=0; i<fromSignature.sensorData().stereoCameraModels().size(); ++i)
1785  {
1786  CameraModel cameraModel = fromSignature.sensorData().stereoCameraModels()[i].left();
1787  // Set Tx=-baseline*fx for Stereo BA
1788  cameraModel = CameraModel(cameraModel.fx(),
1789  cameraModel.fy(),
1790  cameraModel.cx(),
1791  cameraModel.cy(),
1792  cameraModel.localTransform(),
1793  -fromSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
1794  cameraModel.imageSize());
1795  cameraModelsFrom.push_back(cameraModel);
1796  }
1797  }
1798  else
1799  {
1800  cameraModelsFrom = fromSignature.sensorData().cameraModels();
1801  }
1802 
1803  std::vector<CameraModel> cameraModelsTo;
1804  if(toSignature.sensorData().stereoCameraModels().size())
1805  {
1806  for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
1807  {
1808  CameraModel cameraModel = toSignature.sensorData().stereoCameraModels()[i].left();
1809  // Set Tx=-baseline*fx for Stereo BA
1810  cameraModel = CameraModel(cameraModel.fx(),
1811  cameraModel.fy(),
1812  cameraModel.cx(),
1813  cameraModel.cy(),
1814  cameraModel.localTransform(),
1815  -toSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
1816  cameraModel.imageSize());
1817  cameraModelsTo.push_back(cameraModel);
1818  }
1819  }
1820  else
1821  {
1822  cameraModelsTo = toSignature.sensorData().cameraModels();
1823  }
1824 
1825  models.insert(std::make_pair(1, cameraModelsFrom));
1826  models.insert(std::make_pair(2, cameraModelsTo));
1827 
1828  std::map<int, std::map<int, FeatureBA> > wordReferences;
1829  std::set<int> sbaOutliers;
1830  UDEBUG("");
1831  for(unsigned int i=0; i<allInliers.size(); ++i)
1832  {
1833  int wordId = allInliers[i];
1834  int indexFrom = fromSignature.getWords().find(wordId)->second;
1835  const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
1836  if(!util3d::isFinite(pt3D))
1837  {
1838  UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
1839  sbaOutliers.insert(wordId);
1840  continue;
1841  }
1842 
1843  points3DMap.insert(std::make_pair(wordId, pt3D));
1844 
1845  std::map<int, FeatureBA> ptMap;
1846  if(!fromSignature.getWordsKpts().empty())
1847  {
1848  cv::KeyPoint kpt = fromSignature.getWordsKpts()[indexFrom];
1849 
1850  int cameraIndex = 0;
1851  const std::vector<CameraModel> & cam = models.at(1);
1852  if(cam.size()>1)
1853  {
1854  UASSERT(cam[0].imageWidth()>0);
1855  float subImageWidth = cam[0].imageWidth();
1856  cameraIndex = int(kpt.pt.x / subImageWidth);
1857  kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
1858  }
1859 
1860  UASSERT(cam[cameraIndex].isValidForProjection());
1861 
1862  float depthFrom = util3d::transformPoint(pt3D, cam[cameraIndex].localTransform().inverse()).z;
1863  ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom, cv::Mat(), cameraIndex)));
1864  }
1865 
1866  if(!toSignature.getWordsKpts().empty())
1867  {
1868  int indexTo = toSignature.getWords().find(wordId)->second;
1869  cv::KeyPoint kpt = toSignature.getWordsKpts()[indexTo];
1870 
1871  int cameraIndex = 0;
1872  const std::vector<CameraModel> & cam = models.at(2);
1873  if(cam.size()>1)
1874  {
1875  UASSERT(cam[0].imageWidth()>0);
1876  float subImageWidth = cam[0].imageWidth();
1877  cameraIndex = int(kpt.pt.x / subImageWidth);
1878  kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
1879  }
1880 
1881  UASSERT(cam[cameraIndex].isValidForProjection());
1882 
1883  float depthTo = 0.0f;
1884  if(!toSignature.getWords3().empty())
1885  {
1886  depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], cam[cameraIndex].localTransform().inverse()).z;
1887  }
1888 
1889  ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo, cv::Mat(), cameraIndex)));
1890  }
1891 
1892  wordReferences.insert(std::make_pair(wordId, ptMap));
1893 
1894  //UDEBUG("%d (%f,%f,%f)", wordId, points3DMap.at(wordId).x, points3DMap.at(wordId).y, points3DMap.at(wordId).z);
1895  //for(std::map<int, cv::Point3f>::iterator iter=ptMap.begin(); iter!=ptMap.end(); ++iter)
1896  //{
1897  // UDEBUG("%d (%f,%f) d=%f", iter->first, iter->second.x, iter->second.y, iter->second.z);
1898  //}
1899  }
1900 
1901  optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
1902  delete sba;
1903 
1904  //update transform
1905  if(optimizedPoses.size() == 2 &&
1906  !optimizedPoses.begin()->second.isNull() &&
1907  !optimizedPoses.rbegin()->second.isNull())
1908  {
1909  UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
1910 
1911  if(sbaOutliers.size())
1912  {
1913  std::vector<int> newInliers(allInliers.size());
1914  int oi=0;
1915  for(unsigned int i=0; i<allInliers.size(); ++i)
1916  {
1917  if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
1918  {
1919  newInliers[oi++] = allInliers[i];
1920  }
1921  }
1922  newInliers.resize(oi);
1923  UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
1924  allInliers = newInliers;
1925  }
1926  if((int)allInliers.size() < _minInliers)
1927  {
1928  msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
1929  (int)allInliers.size(), _minInliers, (int)allInliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
1930  transforms[0].setNull();
1931  }
1932  else
1933  {
1934  transforms[0] = optimizedPoses.rbegin()->second;
1935  }
1936  // update 3D points, both from and to signatures
1937  /*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
1938  std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
1939  Transform invT = transforms[0].inverse();
1940  for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
1941  {
1942  cpyWordsFrom3.find(iter->first)->second = iter->second;
1943  if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
1944  {
1945  cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
1946  }
1947  }
1948  fromSignature.setWords3(cpyWordsFrom3);
1949  toSignature.setWords3(cpyWordsTo3);*/
1950  }
1951  else
1952  {
1953  transforms[0].setNull();
1954  }
1955  transforms[1].setNull();
1956  }
1957 
1958  info.inliersIDs = allInliers;
1959  info.matchesIDs = allMatches;
1960  inliersCount = (int)allInliers.size();
1961  matchesCount = (int)allMatches.size();
1962  if(!transforms[1].isNull())
1963  {
1964  transforms[1] = transforms[1].inverse();
1965  if(transforms[0].isNull())
1966  {
1967  transform = transforms[1];
1968  covariance = covariances[1];
1969  }
1970  else
1971  {
1972  transform = transforms[0].interpolate(0.5f, transforms[1]);
1973  covariance = (covariances[0]+covariances[1])/2.0f;
1974  }
1975  }
1976  else
1977  {
1978  transform = transforms[0];
1979  covariance = covariances[0];
1980  }
1981 
1982  if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
1983  {
1984  cv::Mat pcaData;
1985  std::vector<CameraModel> cameraModelsTo;
1986  if(toSignature.sensorData().stereoCameraModels().size())
1987  {
1988  for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
1989  {
1990  cameraModelsTo.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
1991  }
1992  }
1993  else
1994  {
1995  cameraModelsTo = toSignature.sensorData().cameraModels();
1996  }
1997  if(_minInliersDistributionThr > 0)
1998  {
1999  if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
2000  {
2001  if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
2002  {
2003  pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
2004  }
2005  else
2006  {
2007  UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
2008  }
2009  }
2010  else
2011  {
2012  UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
2013  }
2014  }
2015 
2016  Transform transformInv = transform.inverse();
2017  std::vector<float> distances;
2018  if(_maxInliersMeanDistance>0.0f)
2019  {
2020  distances.reserve(allInliers.size());
2021  }
2022  for(unsigned int i=0; i<allInliers.size(); ++i)
2023  {
2024  if(_maxInliersMeanDistance>0.0f)
2025  {
2026  std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
2027  if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty())
2028  {
2029  const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second];
2030  if(uIsFinite(pt.x))
2031  {
2032  distances.push_back(util3d::transformPoint(pt, transformInv).x);
2033  }
2034  }
2035  }
2036 
2037  if(!pcaData.empty())
2038  {
2039  std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
2040  UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
2041  float * ptr = pcaData.ptr<float>(i, 0);
2042  const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
2043  int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
2044  UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
2045  ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
2046  ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
2047  }
2048  }
2049 
2050  if(!distances.empty())
2051  {
2052  info.inliersMeanDistance = uMean(distances);
2053 
2054  if(info.inliersMeanDistance > _maxInliersMeanDistance)
2055  {
2056  msg = uFormat("The mean distance of the inliers is over %s threshold (%f)",
2057  info.inliersMeanDistance, Parameters::kVisMeanInliersDistance().c_str(), _maxInliersMeanDistance);
2058  transform.setNull();
2059  }
2060  }
2061 
2062  if(!transform.isNull() && !pcaData.empty())
2063  {
2064  cv::Mat pcaEigenVectors, pcaEigenValues;
2065  cv::PCA pca_analysis(pcaData, cv::Mat(), CV_PCA_DATA_AS_ROW);
2066  // We take the second eigen value
2067  info.inliersDistribution = pca_analysis.eigenvalues.at<float>(0, 1);
2068 
2069  if(info.inliersDistribution < _minInliersDistributionThr)
2070  {
2071  msg = uFormat("The distribution (%f) of inliers is under %s threshold (%f)",
2072  info.inliersDistribution, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
2073  transform.setNull();
2074  }
2075  }
2076  }
2077  }
2078  else if(toSignature.sensorData().isValid())
2079  {
2080  msg = uFormat("Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
2081  fromSignature.id(), toSignature.id(),
2082  (int)fromSignature.getWords().size(), fromSignature.sensorData().imageRaw().empty()?1:0,
2083  (int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
2084  }
2085 
2086  info.inliers = inliersCount;
2087  info.inliersRatio = !toSignature.getWords().empty()?float(inliersCount)/float(toSignature.getWords().size()):0;
2088  info.matches = matchesCount;
2089  info.rejectedMsg = msg;
2090  info.covariance = covariance;
2091 
2092  UDEBUG("inliers=%d/%d", info.inliers, info.matches);
2093  UDEBUG("transform=%s", transform.prettyPrint().c_str());
2094  return transform;
2095 }
2096 
2097 }
rtabmap::Parameters::isFeatureParameter
static bool isFeatureParameter(const std::string &param)
Definition: Parameters.cpp:158
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON
static double COVARIANCE_ANGULAR_EPSILON
Definition: Registration.h:49
rtabmap::Feature2D::getMinDepth
float getMinDepth() const
Definition: Features2d.h:205
cam
rtabmap::CameraThread * cam
Definition: tools/DataRecorder/main.cpp:58
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::RegistrationVis::_bundleParameters
ParametersMap _bundleParameters
Definition: RegistrationVis.h:104
uMultimapToMapUnique
std::map< K, V > uMultimapToMapUnique(const std::multimap< K, V > &m)
Definition: UStl.h:505
UTimer::restart
double restart()
Definition: UTimer.h:94
UTimer
Definition: UTimer.h:46
rtabmap::Feature2D::getType
virtual Feature2D::Type getType() const =0
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
rtabmap::Feature2D::getMaxDepth
float getMaxDepth() const
Definition: Features2d.h:206
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:271
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
f
f
rtabmap::util3d::generateWords3DMono
std::map< int, cv::Point3f > RTABMAP_EXP 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
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
x
x
util3d_motion_estimation.h
rtabmap::VWDictionary::update
virtual void update()
Definition: VWDictionary.cpp:468
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:115
glm::e
GLM_FUNC_DECL genType e()
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
rtabmap::VWDictionary::addNewWords
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
Definition: VWDictionary.cpp:759
rtabmap::Transform::interpolate
Transform interpolate(float t, const Transform &other) const
Definition: Transform.cpp:293
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
glm::distance
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:270
rtabmap::Registration::force3DoF
bool force3DoF() const
Definition: Registration.h:69
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:131
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
UConversion.h
Some conversion functions.
rtabmap::Signature::getWeight
int getWeight() const
Definition: Signature.h:74
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, std::vector< CameraModel > > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:437
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:670
rtabmap::Registration::COVARIANCE_LINEAR_EPSILON
static double COVARIANCE_LINEAR_EPSILON
Definition: Registration.h:48
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::util2d::interpolate
cv::Mat RTABMAP_EXP interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1250
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:112
UASSERT
#define UASSERT(condition)
rtabmap::Feature2D::generateKeypoints3D
std::vector< cv::Point3f > generateKeypoints3D(const SensorData &data, const std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:788
UStl.h
Wrappers of STL for convenient functions.
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:511
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
rtabmap::util3d::estimateMotion3DTo2D
Transform RTABMAP_EXP estimateMotion3DTo2D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::KeyPoint > &words2B, const CameraModel &cameraModel, int minInliers=10, int iterations=100, double reprojError=5., int flagsPnP=0, int pnpRefineIterations=1, float maxVariance=0, const Transform &guess=Transform::getIdentity(), const std::map< int, cv::Point3f > &words3B=std::map< int, cv::Point3f >(), cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
Definition: util3d_motion_estimation.cpp:55
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
rtabmap::RegistrationInfo::projectedIDs
std::vector< int > projectedIDs
Definition: RegistrationInfo.h:87
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:79
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:272
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
rtabmap::Feature2D::filterKeypointsByDepth
static void filterKeypointsByDepth(std::vector< cv::KeyPoint > &keypoints, const cv::Mat &depth, float minDepth, float maxDepth)
Definition: Features2d.cpp:85
rtabmap::VWDictionary::clear
void clear(bool printWarningsIfNotEmpty=true)
Definition: VWDictionary.cpp:692
rtabmap::util3d::estimateMotion3DTo3D
Transform RTABMAP_EXP estimateMotion3DTo3D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::Point3f > &words3B, int minInliers=10, double inliersDistance=0.1, int iterations=100, int refineIterations=5, cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
Definition: util3d_motion_estimation.cpp:461
util3d_transforms.h
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
matches
matches
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:807
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
rtabmap::Feature2D::generateDescriptors
cv::Mat generateDescriptors(const cv::Mat &image, std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:772
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
UDEBUG
#define UDEBUG(...)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
rtabmap::Signature::setWords
void setWords(const std::multimap< int, int > &words, const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &words3, const cv::Mat &descriptors)
Definition: Signature.cpp:262
UERROR
#define UERROR(...)
rtabmap::RegistrationVis::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: RegistrationVis.cpp:114
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::RegistrationVis::computeTransformationImpl
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const
Definition: RegistrationVis.cpp:278
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::RegistrationVis::_featureParameters
ParametersMap _featureParameters
Definition: RegistrationVis.h:103
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:86
trace.model
model
Definition: trace.py:4
matcher
matcher
util3d_features.h
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
rtabmap::VWDictionary::addWord
virtual void addWord(VisualWord *vw)
Definition: VWDictionary.cpp:1393
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
rtabmap::RegistrationVis::RegistrationVis
RegistrationVis(const ParametersMap &parameters=ParametersMap(), Registration *child=0)
Definition: RegistrationVis.cpp:60
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

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