OdometryMono.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 #include "rtabmap/core/odometry/OdometryMono.h"
29 #include "rtabmap/core/OdometryInfo.h"
30 #include "rtabmap/core/Memory.h"
31 #include "rtabmap/core/Signature.h"
32 #include "rtabmap/core/util3d_transforms.h"
33 #include "rtabmap/core/util3d_motion_estimation.h"
34 #include "rtabmap/core/util3d.h"
35 #include "rtabmap/core/util2d.h"
36 #include "rtabmap/core/util3d_features.h"
37 #include "rtabmap/core/EpipolarGeometry.h"
38 #include "rtabmap/core/Optimizer.h"
39 #include "rtabmap/core/Stereo.h"
40 #include "rtabmap/utilite/ULogger.h"
41 #include "rtabmap/utilite/UTimer.h"
42 #include "rtabmap/utilite/UConversion.h"
43 #include "rtabmap/utilite/UStl.h"
44 #include "rtabmap/utilite/UMath.h"
45 #include <opencv2/imgproc/imgproc.hpp>
46 #include <opencv2/calib3d/calib3d.hpp>
47 #include <opencv2/video/tracking.hpp>
48 #include <pcl/common/centroid.h>
49 
50 namespace rtabmap {
51 
52 OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
53  Odometry(parameters),
54  flowWinSize_(Parameters::defaultVisCorFlowWinSize()),
55  flowIterations_(Parameters::defaultVisCorFlowIterations()),
56  flowEps_(Parameters::defaultVisCorFlowEps()),
57  flowMaxLevel_(Parameters::defaultVisCorFlowMaxLevel()),
58  minInliers_(Parameters::defaultVisMinInliers()),
59  iterations_(Parameters::defaultVisIterations()),
60  pnpReprojError_(Parameters::defaultVisPnPReprojError()),
61  pnpFlags_(Parameters::defaultVisPnPFlags()),
62  pnpRefineIterations_(Parameters::defaultVisPnPRefineIterations()),
63  localHistoryMaxSize_(Parameters::defaultOdomF2MMaxSize()),
64  initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
65  initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
66  minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
67  fundMatrixReprojError_(Parameters::defaultVhEpRansacParam1()),
68  fundMatrixConfidence_(Parameters::defaultVhEpRansacParam2()),
69  maxVariance_(Parameters::defaultOdomMonoMaxVariance()),
70  keyFrameThr_(0.95)
71 {
72  Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), flowWinSize_);
73  Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), flowIterations_);
74  Parameters::parse(parameters, Parameters::kVisCorFlowEps(), flowEps_);
75  Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), flowMaxLevel_);
76  Parameters::parse(parameters, Parameters::kVisMinInliers(), minInliers_);
77  UASSERT(minInliers_ >= 1);
78  Parameters::parse(parameters, Parameters::kVisIterations(), iterations_);
79  Parameters::parse(parameters, Parameters::kVisPnPReprojError(), pnpReprojError_);
80  Parameters::parse(parameters, Parameters::kVisPnPFlags(), pnpFlags_);
81  Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), pnpRefineIterations_);
82  Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), localHistoryMaxSize_);
83 
84  Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
85  Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
86  Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
87  Parameters::parse(parameters, Parameters::kOdomMonoMaxVariance(), maxVariance_);
88  Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
89 
90  Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), fundMatrixReprojError_);
91  Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), fundMatrixConfidence_);
92 
93  // Setup memory
94  ParametersMap customParameters;
95  std::string roi = Parameters::defaultVisRoiRatios();
96  Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
97  customParameters.insert(ParametersPair(Parameters::kMemDepthAsMask(), "false"));
98  customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
99  customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
100  customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
101  customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
102  customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
103  customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
104  int nn = Parameters::defaultVisCorNNType();
105  float nndr = Parameters::defaultVisCorNNDR();
106  int featureType = Parameters::defaultVisFeatureType();
107  int maxFeatures = Parameters::defaultVisMaxFeatures();
108  Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
109  Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
110  Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
111  Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
112  customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
113  customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
114  customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
115  customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
116 
117  int subPixWinSize = Parameters::defaultVisSubPixWinSize();
118  int subPixIterations = Parameters::defaultVisSubPixIterations();
119  double subPixEps = Parameters::defaultVisSubPixEps();
120  Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
121  Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
122  Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
123  customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
124  customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
125  customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
126 
127  // add only feature stuff
128  for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
129  {
130  if(Parameters::isFeatureParameter(iter->first))
131  {
132  customParameters.insert(*iter);
133  }
134  }
135 
136  memory_ = new Memory(customParameters);
137  if(!memory_->init("", false, ParametersMap()))
138  {
139  UERROR("Error initializing the memory for Mono Odometry.");
140  }
141 
142  feature2D_ = Feature2D::create(parameters);
143 }
144 
145 OdometryMono::~OdometryMono()
146 {
147  delete memory_;
148  delete feature2D_;
149 }
150 
151 void OdometryMono::reset(const Transform & initialPose)
152 {
153  Odometry::reset(initialPose);
154  memory_->init("", false, ParametersMap());
155  localMap_.clear();
156  firstFrameGuessCorners_.clear();
157  keyFrameWords3D_.clear();
158  keyFramePoses_.clear();
159  keyFrameModels_.clear();
160  keyFrameLinks_.clear();
161 }
162 
163 Transform OdometryMono::computeTransform(SensorData & data, const Transform & guess, OdometryInfo * info)
164 {
165  Transform output;
166 
167  if(data.imageRaw().empty())
168  {
169  UERROR("Image empty! Cannot compute odometry...");
170  return output;
171  }
172 
173  if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection()) || data.stereoCameraModel().isValidForProjection())))
174  {
175  UERROR("Odometry cannot be done without calibration or on multi-camera!");
176  return output;
177  }
178 
179 
180  CameraModel cameraModel;
181  if(data.stereoCameraModel().isValidForProjection())
182  {
183  cameraModel = data.stereoCameraModel().left();
184  // Set Tx for stereo BA
185  cameraModel = CameraModel(cameraModel.fx(),
186  cameraModel.fy(),
187  cameraModel.cx(),
188  cameraModel.cy(),
189  cameraModel.localTransform(),
190  -data.stereoCameraModel().baseline()*cameraModel.fx());
191  }
192  else
193  {
194  cameraModel = data.cameraModels()[0];
195  }
196 
197  UTimer timer;
198 
199  int inliers = 0;
200  int correspondences = 0;
201  int nFeatures = 0;
202 
203  // convert to grayscale
204  if(data.imageRaw().channels() > 1)
205  {
206  cv::Mat newFrame;
207  cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
208  data.setImageRaw(newFrame);
209  }
210 
211  if(!localMap_.empty())
212  {
213  UDEBUG("RUNNING");
214  //PnP
215  UDEBUG("PnP");
216 
217  if(this->isInfoDataFilled() && info)
218  {
219  info->type = 0;
220  }
221 
222  // generate kpts
223  if(memory_->update(data))
224  {
225  UDEBUG("");
226  bool newPtsAdded = false;
227  const Signature * newS = memory_->getLastWorkingSignature();
228  UDEBUG("newWords=%d", (int)newS->getWords().size());
229  nFeatures = (int)newS->getWords().size();
230  if((int)newS->getWords().size() > minInliers_)
231  {
232  cv::Mat K = cameraModel.K();
233  Transform pnpGuess = ((this->getPose() * (guess.isNull()?Transform::getIdentity():guess)) * cameraModel.localTransform()).inverse();
234  cv::Mat R = (cv::Mat_<double>(3,3) <<
235  (double)pnpGuess.r11(), (double)pnpGuess.r12(), (double)pnpGuess.r13(),
236  (double)pnpGuess.r21(), (double)pnpGuess.r22(), (double)pnpGuess.r23(),
237  (double)pnpGuess.r31(), (double)pnpGuess.r32(), (double)pnpGuess.r33());
238  cv::Mat rvec(1,3, CV_64FC1);
239  cv::Rodrigues(R, rvec);
240  cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)pnpGuess.x(), (double)pnpGuess.y(), (double)pnpGuess.z());
241 
242  std::vector<cv::Point3f> objectPoints;
243  std::vector<cv::Point2f> imagePoints;
244  std::vector<int> matches;
245 
246  UDEBUG("compute PnP from optical flow");
247 
248  std::vector<int> ids = uKeys(localMap_);
249  objectPoints = uValues(localMap_);
250 
251  // compute last projection
252  UDEBUG("project points to previous image");
253  std::vector<cv::Point2f> prevImagePoints;
254  const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
255  Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse();
256  cv::Mat prevR = (cv::Mat_<double>(3,3) <<
257  (double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
258  (double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
259  (double)prevGuess.r31(), (double)prevGuess.r32(), (double)prevGuess.r33());
260  cv::Mat prevRvec(1,3, CV_64FC1);
261  cv::Rodrigues(prevR, prevRvec);
262  cv::Mat prevTvec = (cv::Mat_<double>(1,3) << (double)prevGuess.x(), (double)prevGuess.y(), (double)prevGuess.z());
263  cv::projectPoints(objectPoints, prevRvec, prevTvec, K, cv::Mat(), prevImagePoints);
264 
265  // compute current projection
266  UDEBUG("project points to current image");
267  cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePoints);
268 
269  //filter points not in the image and set guess from unique correspondences
270  std::vector<cv::Point3f> objectPointsTmp(objectPoints.size());
271  std::vector<cv::Point2f> refCorners(objectPoints.size());
272  std::vector<cv::Point2f> newCorners(objectPoints.size());
273  matches.resize(objectPoints.size());
274  int oi=0;
275  for(unsigned int i=0; i<objectPoints.size(); ++i)
276  {
277  if(uIsInBounds(int(imagePoints[i].x), 0, newS->sensorData().imageRaw().cols) &&
278  uIsInBounds(int(imagePoints[i].y), 0, newS->sensorData().imageRaw().rows) &&
279  uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) &&
280  uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows))
281  {
282  refCorners[oi] = prevImagePoints[i];
283  newCorners[oi] = imagePoints[i];
284  if(localMap_.count(ids[i]) == 1)
285  {
286  if(prevS->getWords().count(ids[i]) == 1 && !prevS->getWordsKpts().empty())
287  {
288  // set guess if unique
289  refCorners[oi] = prevS->getWordsKpts()[prevS->getWords().find(ids[i])->second].pt;
290  }
291  if(newS->getWords().count(ids[i]) == 1 && !newS->getWordsKpts().empty())
292  {
293  // set guess if unique
294  newCorners[oi] = newS->getWordsKpts()[newS->getWords().find(ids[i])->second].pt;
295  }
296  }
297  objectPointsTmp[oi] = objectPoints[i];
298  matches[oi] = ids[i];
299  ++oi;
300  }
301  }
302  objectPointsTmp.resize(oi);
303  refCorners.resize(oi);
304  newCorners.resize(oi);
305  matches.resize(oi);
306 
307  // Refine imagePoints using optical flow
308  std::vector<unsigned char> statusFlowInliers;
309  std::vector<float> err;
310  UDEBUG("cv::calcOpticalFlowPyrLK() begin");
311  cv::calcOpticalFlowPyrLK(
312  prevS->sensorData().imageRaw(),
313  newS->sensorData().imageRaw(),
314  refCorners,
315  newCorners,
316  statusFlowInliers,
317  err,
318  cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
319  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
320  cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
321  UDEBUG("cv::calcOpticalFlowPyrLK() end");
322 
323  objectPoints.resize(statusFlowInliers.size());
324  imagePoints.resize(statusFlowInliers.size());
325  std::vector<int> matchesTmp(statusFlowInliers.size());
326  oi = 0;
327  for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
328  {
329  if(statusFlowInliers[i] &&
330  uIsInBounds(int(newCorners[i].x), 0, newS->sensorData().imageRaw().cols) &&
331  uIsInBounds(int(newCorners[i].y), 0, newS->sensorData().imageRaw().rows))
332  {
333  objectPoints[oi] = objectPointsTmp[i];
334  imagePoints[oi] = newCorners[i];
335  matchesTmp[oi] = matches[i];
336  ++oi;
337 
338  if(this->isInfoDataFilled() && info)
339  {
340  cv::KeyPoint kpt;
341  if(newS->getWords().count(matches[i]) == 1 && !newS->getWordsKpts().empty())
342  {
343  kpt = newS->getWordsKpts()[newS->getWords().find(matches[i])->second];
344  }
345  kpt.pt = newCorners[i];
346  info->words.insert(std::make_pair(matches[i], kpt));
347  }
348  }
349  }
350  UDEBUG("Flow inliers= %d/%d", oi, (int)statusFlowInliers.size());
351  objectPoints.resize(oi);
352  imagePoints.resize(oi);
353  matchesTmp.resize(oi);
354  matches = matchesTmp;
355 
356  if(this->isInfoDataFilled() && info)
357  {
358  info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end());
359  }
360  correspondences = (int)matches.size();
361 
362  if((int)matches.size() < minInliers_)
363  {
364  UWARN("not enough matches (%d < %d)...", (int)matches.size(), minInliers_);
365  }
366  else
367  {
368  //PnPRansac
369  std::vector<int> inliersV;
370  util3d::solvePnPRansac(
371  objectPoints,
372  imagePoints,
373  K,
374  cv::Mat(),
375  rvec,
376  tvec,
377  true,
378  iterations_,
379  pnpReprojError_,
380  0, // min inliers
381  inliersV,
382  pnpFlags_,
383  pnpRefineIterations_);
384 
385  UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
386 
387  inliers = (int)inliersV.size();
388  if((int)inliersV.size() < minInliers_)
389  {
390  UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), minInliers_);
391  }
392  else
393  {
394  cv::Mat R(3,3,CV_64FC1);
395  cv::Rodrigues(rvec, R);
396  Transform pnp = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
397  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
398  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
399  output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse();
400 
401  if(this->isInfoDataFilled() && info && inliersV.size())
402  {
403  info->reg.inliersIDs.resize(inliersV.size());
404  for(unsigned int i=0; i<inliersV.size(); ++i)
405  {
406  info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
407  }
408  }
409 
410  // compute variance, which is the rms of reprojection errors
411  cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
412  std::vector<cv::Point2f> imagePointsReproj;
413  cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePointsReproj);
414  float err = 0.0f;
415  for(unsigned int i=0; i<inliersV.size(); ++i)
416  {
417  err += uNormSquared(imagePoints.at(inliersV[i]).x - imagePointsReproj.at(inliersV[i]).x, imagePoints.at(inliersV[i]).y - imagePointsReproj.at(inliersV[i]).y);
418  }
419  UASSERT(uIsFinite(err));
420  covariance *= std::sqrt(err/float(inliersV.size()));
421 
422  Link newLink(keyFramePoses_.rbegin()->first, newS->id(), Link::kNeighbor, output, covariance.inv());
423 
424  //bundle adjustment
425  Optimizer * ba = Optimizer::create(Optimizer::kTypeG2O);
426  std::map<int, Transform> poses = keyFramePoses_;
427  poses.insert(std::make_pair(newS->id(), this->getPose()*output));
428  if(ba->type() == Optimizer::kTypeG2O)
429  {
430  UWARN("Bundle adjustment: fill arguments");
431  std::multimap<int, Link> links = keyFrameLinks_;
432  std::map<int, CameraModel> models = keyFrameModels_;
433  links.insert(std::make_pair(keyFramePoses_.rbegin()->first, newLink));
434  models.insert(std::make_pair(newS->id(), cameraModel));
435  std::map<int, std::map<int, FeatureBA> > wordReferences;
436 
437  for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
438  {
439  const Signature * s = memory_->getSignature(*iter);
440  for(std::multimap<int, int>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
441  {
442  if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end() && !s->getWordsKpts().empty())
443  {
444  if(wordReferences.find(jter->first)==wordReferences.end())
445  {
446  wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
447  }
448  float depth = 0.0f;
449  if(keyFrameWords3D_.find(s->id()) != keyFrameWords3D_.end() &&
450  keyFrameWords3D_.at(s->id()).find(jter->first) != keyFrameWords3D_.at(s->id()).end())
451  {
452  depth = keyFrameWords3D_.at(s->id()).at(jter->first).x;
453  }
454  const cv::KeyPoint & kpts = s->getWordsKpts()[jter->second];
455  wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(kpts, depth, cv::Mat())));
456  }
457  }
458  }
459 
460  std::set<int> outliers;
461  UWARN("Bundle adjustment begin");
462  poses = ba->optimizeBA(poses.begin()->first, poses, links, models, localMap_, wordReferences, &outliers);
463  UWARN("Bundle adjustment end");
464  if(!poses.empty())
465  {
466  output = this->getPose().inverse()*poses.at(newS->id());
467  }
468  }
469 
470 
471  //Find the frame with the most similar features
472  std::set<int> stMem = memory_->getStMem();
473  stMem.erase(newS->id());
474  std::map<int, float> likelihood = memory_->computeLikelihood(newS, std::list<int>(stMem.begin(), stMem.end()));
475  int maxLikelihoodId = -1;
476  float maxLikelihood = 0;
477  for(std::map<int, float>::iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
478  {
479  if(iter->second > maxLikelihood)
480  {
481  maxLikelihood = iter->second;
482  maxLikelihoodId = iter->first;
483  }
484  }
485  if(maxLikelihoodId == -1)
486  {
487  UWARN("Cannot find a keyframe similar enough to generate new 3D points!");
488  }
489  else if(poses.size())
490  {
491  // Add new points to local map
492  const Signature* previousS = memory_->getSignature(maxLikelihoodId);
493  UASSERT(previousS!=0);
494  Transform cameraTransform = keyFramePoses_.at(previousS->id()).inverse()*this->getPose()*output;
495  UDEBUG("cameraTransform guess= %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
496 
497  UINFO("Inliers= %d/%d (%f)", inliers, (int)imagePoints.size(), float(inliers)/float(imagePoints.size()));
498 
499  if(cameraTransform.getNorm() < minTranslation_)
500  {
501  UINFO("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
502  cameraTransform.getNorm(), minTranslation_);
503  }
504  else if(float(inliers)/float(imagePoints.size()) < keyFrameThr_)
505  {
506  std::map<int, int> uniqueWordsPrevious = uMultimapToMapUnique(previousS->getWords());
507  std::map<int, int> uniqueWordsNew = uMultimapToMapUnique(newS->getWords());
508  std::map<int, cv::KeyPoint> wordsPrevious;
509  std::map<int, cv::KeyPoint> wordsNew;
510  for(std::map<int, int>::iterator iter=uniqueWordsPrevious.begin(); iter!=uniqueWordsPrevious.end(); ++iter)
511  {
512  wordsPrevious.insert(std::make_pair(iter->first, previousS->getWordsKpts()[iter->second]));
513  }
514  for(std::map<int, int>::iterator iter=uniqueWordsNew.begin(); iter!=uniqueWordsNew.end(); ++iter)
515  {
516  wordsNew.insert(std::make_pair(iter->first, newS->getWordsKpts()[iter->second]));
517  }
518  std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
519  wordsPrevious,
520  wordsNew,
521  cameraModel,
522  cameraTransform,
523  fundMatrixReprojError_,
524  fundMatrixConfidence_);
525 
526  if((int)inliers3D.size() < minInliers_)
527  {
528  UWARN("Epipolar geometry not enough inliers (%d < %d), rejecting the transform (%s)...",
529  (int)inliers3D.size(), minInliers_, cameraTransform.prettyPrint().c_str());
530  }
531  else
532  {
533  Transform newPose = keyFramePoses_.at(previousS->id())*cameraTransform;
534  UDEBUG("cameraTransform= %s", cameraTransform.prettyPrint().c_str());
535 
536  std::multimap<int, cv::Point3f> wordsToAdd;
537  for(std::map<int, cv::Point3f>::iterator iter=inliers3D.begin();
538  iter != inliers3D.end();
539  ++iter)
540  {
541  // transform inliers3D in new signature referential
542  iter->second = util3d::transformPoint(iter->second, cameraTransform.inverse());
543 
544  if(!uContains(localMap_, iter->first))
545  {
546  //UDEBUG("Add new point %d to local map", iter->first);
547  cv::Point3f newPt = util3d::transformPoint(iter->second, newPose);
548  wordsToAdd.insert(std::make_pair(iter->first, newPt));
549  }
550  }
551 
552  if((int)wordsToAdd.size())
553  {
554  localMap_.insert(wordsToAdd.begin(), wordsToAdd.end());
555  newPtsAdded = true;
556  UWARN("Added %d words", (int)wordsToAdd.size());
557  }
558 
559  if(newPtsAdded)
560  {
561  // if we have depth guess, set it for ba
562  std::vector<cv::Point3f> newCorners3;
563  if(!data.depthOrRightRaw().empty())
564  {
565  std::vector<cv::KeyPoint> newKeypoints(imagePoints.size());
566  for(size_t i=0;i<imagePoints.size(); ++i)
567  {
568  newKeypoints[i] = cv::KeyPoint(imagePoints[i], 3);
569  }
570  newCorners3 = feature2D_->generateKeypoints3D(data, newKeypoints);
571  for(size_t i=0;i<newCorners3.size(); ++i)
572  {
573  if(util3d::isFinite(newCorners3[i]) &&
574  inliers3D.find(matches[i])!=inliers3D.end())
575  {
576  inliers3D.at(matches[i]) = newCorners3[i];
577  }
578  else
579  {
580  inliers3D.erase(matches[i]);
581  }
582  }
583  }
584 
585  if(!inliers3D.empty())
586  {
587  UDEBUG("Added %d/%d valid 3D features", (int)inliers3D.size(), (int)wordsToAdd.size());
588  keyFrameWords3D_.insert(std::make_pair(newS->id(), inliers3D));
589  }
590  keyFramePoses_ = poses;
591  keyFrameLinks_.insert(std::make_pair(newLink.from(), newLink));
592  keyFrameModels_.insert(std::make_pair(newS->id(), cameraModel));
593 
594  // keep only the two last signatures
595  while(localHistoryMaxSize_ && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>2)
596  {
597  int nodeId = *memory_->getStMem().begin();
598  std::list<int> removedPts;
599  memory_->deleteLocation(nodeId, &removedPts);
600  keyFrameWords3D_.erase(nodeId);
601  keyFramePoses_.erase(nodeId);
602  keyFrameLinks_.erase(nodeId);
603  keyFrameModels_.erase(nodeId);
604  for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
605  {
606  localMap_.erase(*iter);
607  }
608  }
609  }
610  }
611  }
612  }
613  }
614  }
615  }
616 
617  if(!newPtsAdded)
618  {
619  // remove new words from dictionary
620  memory_->deleteLocation(newS->id());
621  }
622  }
623  }
624  else if(!firstFrameGuessCorners_.empty())
625  {
626  UDEBUG("INIT PART 2/2");
627  //flow
628 
629  if(this->isInfoDataFilled() && info)
630  {
631  info->type = 1;
632  }
633 
634  const Signature * refS = memory_->getLastWorkingSignature();
635 
636  std::vector<cv::Point2f> refCorners(firstFrameGuessCorners_.size());
637  std::vector<cv::Point2f> refCornersGuess(firstFrameGuessCorners_.size());
638  std::vector<int> cornerIds(firstFrameGuessCorners_.size());
639  int ii=0;
640  for(std::map<int, cv::Point2f>::iterator iter=firstFrameGuessCorners_.begin(); iter!=firstFrameGuessCorners_.end(); ++iter)
641  {
642  std::multimap<int, int>::const_iterator jter=refS->getWords().find(iter->first);
643  UASSERT(jter != refS->getWords().end() && !refS->getWordsKpts().empty());
644  refCorners[ii] = refS->getWordsKpts()[jter->second].pt;
645  refCornersGuess[ii] = iter->second;
646  cornerIds[ii] = iter->first;
647  ++ii;
648  }
649 
650  UDEBUG("flow");
651  // Find features in the new left image
652  std::vector<unsigned char> statusFlowInliers;
653  std::vector<float> err;
654  UDEBUG("cv::calcOpticalFlowPyrLK() begin");
655  cv::calcOpticalFlowPyrLK(
656  refS->sensorData().imageRaw(),
657  data.imageRaw(),
658  refCorners,
659  refCornersGuess,
660  statusFlowInliers,
661  err,
662  cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
663  cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
664  cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
665  UDEBUG("cv::calcOpticalFlowPyrLK() end");
666 
667  UDEBUG("Filtering optical flow outliers...");
668  float flow = 0;
669 
670  if(this->isInfoDataFilled() && info)
671  {
672  info->refCorners = refCorners;
673  info->newCorners = refCornersGuess;
674  }
675 
676  int oi = 0;
677  std::vector<cv::Point2f> tmpRefCorners(statusFlowInliers.size());
678  std::vector<cv::Point2f> newCorners(statusFlowInliers.size());
679  std::vector<int> inliersV(statusFlowInliers.size());
680  std::vector<int> tmpCornersId(statusFlowInliers.size());
681  UASSERT(refCornersGuess.size() == statusFlowInliers.size());
682  UASSERT(refCorners.size() == statusFlowInliers.size());
683  UASSERT(cornerIds.size() == statusFlowInliers.size());
684  for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
685  {
686  if(statusFlowInliers[i])
687  {
688  float dx = refCorners[i].x - refCornersGuess[i].x;
689  float dy = refCorners[i].y - refCornersGuess[i].y;
690  float tmp = std::sqrt(dx*dx + dy*dy);
691  flow+=tmp;
692 
693  tmpRefCorners[oi] = refCorners[i];
694  newCorners[oi] = refCornersGuess[i];
695 
696  firstFrameGuessCorners_.at(cornerIds[i]) = refCornersGuess[i];
697 
698  inliersV[oi] = i;
699  tmpCornersId[oi] = cornerIds[i];
700 
701  ++oi;
702  }
703  else
704  {
705  firstFrameGuessCorners_.erase(cornerIds[i]);
706  }
707  }
708  if(oi)
709  {
710  flow /=float(oi);
711  }
712  tmpRefCorners.resize(oi);
713  newCorners.resize(oi);
714  inliersV.resize((oi));
715  tmpCornersId.resize(oi);
716  refCorners= tmpRefCorners;
717  cornerIds = tmpCornersId;
718 
719  if(this->isInfoDataFilled() && info)
720  {
721  // fill flow matches info
722  info->cornerInliers = inliersV;
723  inliers = (int)inliersV.size();
724  }
725 
726  UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)statusFlowInliers.size());
727 
728  if(flow > initMinFlow_ && oi > minInliers_)
729  {
730  UDEBUG("flow=%f", flow);
731  std::vector<cv::Point3f> refCorners3;
732  if(!refS->sensorData().depthOrRightRaw().empty())
733  {
734  std::vector<cv::KeyPoint> refKeypoints(refCorners.size());
735  for(size_t i=0;i<refCorners.size(); ++i)
736  {
737  refKeypoints[i] = cv::KeyPoint(refCorners[i], 3);
738  }
739  refCorners3 = feature2D_->generateKeypoints3D(refS->sensorData(), refKeypoints);
740  }
741 
742  std::map<int, cv::KeyPoint> refWords;
743  std::map<int, cv::KeyPoint> newWords;
744  std::map<int, cv::Point3f> refWords3Guess;
745  for(unsigned int i=0; i<cornerIds.size(); ++i)
746  {
747  refWords.insert(std::make_pair(cornerIds[i], cv::KeyPoint(refCorners[i], 3)));
748  newWords.insert(std::make_pair(cornerIds[i], cv::KeyPoint(newCorners[i], 3)));
749  if(!refCorners3.empty())
750  {
751  refWords3Guess.insert(std::make_pair(cornerIds[i], refCorners3[i]));
752  }
753  }
754 
755  Transform cameraTransform;
756  std::map<int, cv::Point3f> refWords3 = util3d::generateWords3DMono(
757  refWords,
758  newWords,
759  cameraModel,
760  cameraTransform,
761  fundMatrixReprojError_,
762  fundMatrixConfidence_,
763  refWords3Guess); // for scale estimation
764 
765  if(cameraTransform.getNorm() < minTranslation_*5)
766  {
767  UWARN("Camera must be moved at least %f m for initialization (current=%f)",
768  minTranslation_*5, output.getNorm());
769  }
770  else
771  {
772  localMap_ = refWords3;
773  // For values that we know the depth, set them for ba
774  for(std::map<int, cv::Point3f>::iterator iter=refWords3.begin(); iter!=refWords3.end();)
775  {
776  std::map<int, cv::Point3f>::iterator jterGuess3D = refWords3Guess.find(iter->first);
777  if(jterGuess3D != refWords3Guess.end() &&
778  util3d::isFinite(jterGuess3D->second))
779  {
780  iter->second = jterGuess3D->second;
781  ++iter;
782  }
783  else
784  {
785  refWords3.erase(iter++);
786  }
787  }
788  if(!refWords3.empty())
789  {
790  UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size());
791  keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3));
792  }
793  keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
794  keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), cameraModel));
795  }
796  }
797  else
798  {
799  UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
800  }
801  }
802  else
803  {
804  UDEBUG("INIT PART 1/2");
805  //return Identity
806  output = Transform::getIdentity();
807  if(info)
808  {
809  // a very high variance tells that the new pose is not linked with the previous one
810  info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
811  }
812 
813  // generate kpts
814  if(memory_->update(SensorData(data)))
815  {
816  const Signature * s = memory_->getLastWorkingSignature();
817  const std::multimap<int, int> & words = s->getWords();
818  if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
819  {
820  for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
821  {
822  if(words.count(iter->first) == 1)
823  {
824  firstFrameGuessCorners_.insert(std::make_pair(iter->first, s->getWordsKpts()[iter->second].pt));
825  }
826  }
827  }
828  else
829  {
830  memory_->deleteLocation(memory_->getLastSignatureId());
831  }
832  }
833  else
834  {
835  UERROR("Failed creating signature");
836  }
837  }
838 
839  memory_->emptyTrash();
840 
841  if(this->isInfoDataFilled() && info)
842  {
843  //info->variance = variance;
844  info->reg.inliers = inliers;
845  info->reg.matches = correspondences;
846  info->features = nFeatures;
847  info->localMapSize = (int)localMap_.size();
848  info->localMap = localMap_;
849  }
850 
851  UINFO("Odom update=%fs tf=[%s] inliers=%d/%d, local_map[%d]=%d, accepted=%s",
852  timer.elapsed(),
853  output.prettyPrint().c_str(),
854  inliers,
855  correspondences,
856  (int)memory_->getStMem().size(),
857  (int)localMap_.size(),
858  !output.isNull()?"true":"false");
859 
860  return output;
861 }
862 
863 } // namespace rtabmap
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::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
uMultimapToMapUnique
std::map< K, V > uMultimapToMapUnique(const std::multimap< K, V > &m)
Definition: UStl.h:505
rtabmap::OdometryInfo::refCorners
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:128
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::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:295
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
rtabmap::OdometryMono::firstFrameGuessCorners_
std::map< int, cv::Point2f > firstFrameGuessCorners_
Definition: OdometryMono.h:71
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:74
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::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:191
util3d_motion_estimation.h
rtabmap::Memory::getSignature
const Signature * getSignature(int id) const
Definition: Memory.cpp:1147
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::OdometryMono::computeTransform
virtual Transform computeTransform(SensorData &data, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryMono.cpp:163
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:124
rtabmap::OdometryMono::feature2D_
Feature2D * feature2D_
Definition: OdometryMono.h:61
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::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:252
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::OdometryMono::OdometryMono
OdometryMono(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryMono.cpp:52
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:73
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:81
UConversion.h
Some conversion functions.
rtabmap::Memory::deleteLocation
void deleteLocation(int locationId, std::list< int > *deletedWords=0)
Definition: Memory.cpp:2570
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
rtabmap::Memory::getStMem
const std::set< int > & getStMem() const
Definition: Memory.h:148
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:112
rtabmap::Memory::init
bool init(const std::string &dbUrl, bool dbOverwritten=false, const ParametersMap &parameters=ParametersMap(), bool postInitClosingEvents=false)
Definition: Memory.cpp:151
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:55
UASSERT
#define UASSERT(condition)
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
rtabmap::OdometryInfo::cornerInliers
std::vector< int > cornerInliers
Definition: OdometryInfo.h:130
rtabmap::OdometryMono::keyFrameModels_
std::map< int, CameraModel > keyFrameModels_
Definition: OdometryMono.h:76
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
rtabmap::Memory::update
bool update(const SensorData &data, Statistics *stats=0)
Definition: Memory.cpp:759
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::Memory::emptyTrash
void emptyTrash()
Definition: Memory.cpp:2116
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:165
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
util3d_transforms.h
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::util3d::solvePnPRansac
void RTABMAP_EXP solvePnPRansac(const std::vector< cv::Point3f > &objectPoints, const std::vector< cv::Point2f > &imagePoints, const cv::Mat &cameraMatrix, const cv::Mat &distCoeffs, cv::Mat &rvec, cv::Mat &tvec, bool useExtrinsicGuess, int iterationsCount, float reprojectionError, int minInliersCount, std::vector< int > &inliers, int flags, int refineIterations=1, float refineSigma=3.0f)
Definition: util3d_motion_estimation.cpp:331
rtabmap::OdometryMono::keyFramePoses_
std::map< int, Transform > keyFramePoses_
Definition: OdometryMono.h:74
rtabmap::Memory::getLastSignatureId
int getLastSignatureId() const
Definition: Memory.cpp:2449
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:123
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
uNormSquared
T uNormSquared(const std::vector< T > &v)
Definition: UMath.h:560
rtabmap::Memory::getLastWorkingSignature
const Signature * getLastWorkingSignature() const
Definition: Memory.cpp:2454
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
UERROR
#define UERROR(...)
rtabmap::Transform::getNormSquared
float getNormSquared() const
Definition: Transform.cpp:257
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
UWARN
#define UWARN(...)
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:83
rtabmap::Memory::computeLikelihood
std::map< int, float > computeLikelihood(const Signature *signature, const std::list< int > &ids)
Definition: Memory.cpp:1795
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:129
rtabmap::OdometryMono::reset
virtual void reset(const Transform &initialPose)
Definition: OdometryMono.cpp:151
util3d_features.h
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:96
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:110
rtabmap::OdometryMono::keyFrameWords3D_
std::map< int, std::map< int, cv::Point3f > > keyFrameWords3D_
Definition: OdometryMono.h:73
rtabmap::OdometryMono::keyFrameLinks_
std::multimap< int, Link > keyFrameLinks_
Definition: OdometryMono.h:75
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
rtabmap::OdometryMono::localMap_
std::map< int, cv::Point3f > localMap_
Definition: OdometryMono.h:72
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
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)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59