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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:32