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