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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00