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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13