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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:58