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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:51