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