OdometryF2M.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/OdometryInfo.h"
29 #include "rtabmap/core/Memory.h"
30 #include "rtabmap/core/Signature.h"
31 #include "rtabmap/core/RegistrationVis.h"
32 #include "rtabmap/core/util3d.h"
33 #include "rtabmap/core/util3d_transforms.h"
34 #include "rtabmap/core/util3d_registration.h"
35 #include "rtabmap/core/util3d_motion_estimation.h"
36 #include "rtabmap/core/util3d_filtering.h"
37 #include "rtabmap/core/util3d_surface.h"
38 #include "rtabmap/core/Optimizer.h"
39 #include "rtabmap/core/VWDictionary.h"
40 #include "rtabmap/core/Graph.h"
41 #include "rtabmap/utilite/ULogger.h"
42 #include "rtabmap/utilite/UTimer.h"
43 #include "rtabmap/utilite/UMath.h"
44 #include "rtabmap/utilite/UConversion.h"
45 #include <opencv2/calib3d/calib3d.hpp>
46 #include <rtabmap/core/odometry/OdometryF2M.h>
47 #include <pcl/common/io.h>
48 
49 #if _MSC_VER
50  #define ISFINITE(value) _finite(value)
51 #else
52  #define ISFINITE(value) std::isfinite(value)
53 #endif
54 
55 namespace rtabmap {
56 
57 OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
58  Odometry(parameters),
59  maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
60  keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
61  visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
62  maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
63  scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
64  scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
65  scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
66  scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
67  scanMapMaxRange_(Parameters::defaultOdomF2MScanRange()),
68  bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
69  bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
70  validDepthRatio_(Parameters::defaultOdomF2MValidDepthRatio()),
71  pointToPlaneK_(Parameters::defaultIcpPointToPlaneK()),
72  pointToPlaneRadius_(Parameters::defaultIcpPointToPlaneRadius()),
73  map_(new Signature(-1)),
74  lastFrame_(new Signature(1)),
75  lastFrameOldestNewId_(0),
76  bundleSeq_(0),
77  sba_(0)
78 {
79  UDEBUG("");
80  Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
81  Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
82  Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
83  Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
84  Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
85  Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
86  Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
87  if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
88  {
89  scanSubtractAngle_ *= M_PI/180.0f;
90  }
91  Parameters::parse(parameters, Parameters::kOdomF2MScanRange(), scanMapMaxRange_);
92  Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
93  Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
94  Parameters::parse(parameters, Parameters::kOdomF2MValidDepthRatio(), validDepthRatio_);
95 
96  Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), pointToPlaneK_);
97  Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), pointToPlaneRadius_);
98 
99  UASSERT(bundleMaxFrames_ >= 0);
100  ParametersMap bundleParameters = parameters;
101  if(bundleAdjustment_ > 0)
102  {
103  if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
104  (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)) ||
105  (bundleAdjustment_==3 && Optimizer::isAvailable(Optimizer::kTypeCeres)))
106  {
107  // disable bundle in RegistrationVis as we do it already here
108  uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
109  sba_ = Optimizer::create(bundleAdjustment_==3?Optimizer::kTypeCeres:bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
110  }
111  else
112  {
113  UWARN("Selected bundle adjustment approach (\"%s\"=\"%d\") is not available, "
114  "local bundle adjustment is then disabled.", Parameters::kOdomF2MBundleAdjustment().c_str(), bundleAdjustment_);
115  bundleAdjustment_ = 0;
116  }
117  }
118  UASSERT(maximumMapSize_ >= 0);
119  UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
120  UASSERT(visKeyFrameThr_>=0);
121  UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
122  UASSERT(maxNewFeatures_ >= 0);
123 
124  int corType = Parameters::defaultVisCorType();
125  Parameters::parse(parameters, Parameters::kVisCorType(), corType);
126  if(corType != 0)
127  {
128  UWARN("%s=%d is not supported by OdometryF2M, using Features matching approach instead (type=0).",
129  Parameters::kVisCorType().c_str(),
130  corType);
131  corType = 0;
132  }
133  uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
134 
135  int estType = Parameters::defaultVisEstimationType();
136  Parameters::parse(parameters, Parameters::kVisEstimationType(), estType);
137  if(estType > 1)
138  {
139  UWARN("%s=%d is not supported by OdometryF2M, using 2D->3D approach instead (type=1).",
140  Parameters::kVisEstimationType().c_str(),
141  estType);
142  estType = 1;
143  }
144  uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
145 
146  bool forwardEst = Parameters::defaultVisForwardEstOnly();
147  Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
148  if(!forwardEst)
149  {
150  UWARN("%s=false is not supported by OdometryF2M, setting to true.",
151  Parameters::kVisForwardEstOnly().c_str());
152  forwardEst = true;
153  }
154  uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
155 
156  regPipeline_ = Registration::create(bundleParameters);
157  if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
158  {
159  UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
160  Parameters::kOdomF2MBundleAdjustment().c_str(),
161  bundleAdjustment_,
162  Parameters::kRegStrategy().c_str(),
163  uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
164  bundleAdjustment_ = 0;
165  }
166 
167  parameters_ = bundleParameters;
168 }
169 
170 OdometryF2M::~OdometryF2M()
171 {
172  delete map_;
173  delete lastFrame_;
174  delete sba_;
175  delete regPipeline_;
176  UDEBUG("");
177 }
178 
179 
180 void OdometryF2M::reset(const Transform & initialPose)
181 {
182  Odometry::reset(initialPose);
183 
184  UDEBUG("initialPose=%s", initialPose.prettyPrint().c_str());
185  Odometry::reset(initialPose);
186  *lastFrame_ = Signature(1);
187  *map_ = Signature(-1);
188  scansBuffer_.clear();
189  bundleWordReferences_.clear();
190  bundlePoses_.clear();
191  bundleLinks_.clear();
192  bundleModels_.clear();
193  bundlePoseReferences_.clear();
194  bundleSeq_ = 0;
195  lastFrameOldestNewId_ = 0;
196 }
197 
198 // return not null transform if odometry is correctly computed
199 Transform OdometryF2M::computeTransform(
200  SensorData & data,
201  const Transform & guessIn,
202  OdometryInfo * info)
203 {
204  Transform guess = guessIn;
205  UTimer timer;
206  Transform output;
207 
208  if(info)
209  {
210  info->type = 0;
211  }
212 
213  Transform imuT;
214  if(sba_ && sba_->gravitySigma() > 0.0f && !imus().empty())
215  {
216  imuT = Transform::getTransform(imus(), data.stamp());
217  }
218 
219  RegistrationInfo regInfo;
220  int nFeatures = 0;
221 
222  delete lastFrame_;
223  int id = data.id();
224  data.setId(++bundleSeq_); // generate our own unique ids, to make sure they are correctly set
225  lastFrame_ = new Signature(data);
226  data.setId(id);
227 
228  bool addKeyFrame = false;
229  int totalBundleWordReferencesUsed = 0;
230  int totalBundleOutliers = 0;
231  float bundleTime = 0.0f;
232  bool visDepthAsMask = Parameters::defaultVisDepthAsMask();
233  Parameters::parse(parameters_, Parameters::kVisDepthAsMask(), visDepthAsMask);
234 
235  std::vector<CameraModel> lastFrameModels;
236  if(!lastFrame_->sensorData().cameraModels().empty() &&
237  lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
238  {
239  lastFrameModels = lastFrame_->sensorData().cameraModels();
240  }
241  else if(!lastFrame_->sensorData().stereoCameraModels().empty() &&
242  lastFrame_->sensorData().stereoCameraModels().at(0).isValidForProjection())
243  {
244  for(size_t i=0; i<lastFrame_->sensorData().stereoCameraModels().size(); ++i)
245  {
246  CameraModel model = lastFrame_->sensorData().stereoCameraModels()[i].left();
247  // Set Tx for stereo BA
248  model = CameraModel(model.fx(),
249  model.fy(),
250  model.cx(),
251  model.cy(),
252  model.localTransform(),
253  -lastFrame_->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
254  model.imageSize());
255  lastFrameModels.push_back(model);
256  }
257  }
258  UDEBUG("lastFrameModels=%ld", lastFrameModels.size());
259 
260  // Generate keypoints from the new data
261  if(lastFrame_->sensorData().isValid())
262  {
263  if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().isEmpty()) &&
264  lastFrame_->sensorData().isValid())
265  {
266  Signature tmpMap;
267  Transform transform;
268  UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
269 
270  // bundle adjustment stuff if used
271  std::map<int, cv::Point3f> points3DMap;
272  std::map<int, Transform> bundlePoses;
273  std::multimap<int, Link> bundleLinks;
274  std::map<int, std::vector<CameraModel> > bundleModels;
275 
276  for(int guessIteration=0;
277  guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
278  ++guessIteration)
279  {
280  tmpMap = *map_;
281  // reset matches, but keep already extracted features in lastFrame_->sensorData()
282  lastFrame_->removeAllWords();
283 
284  points3DMap.clear();
285  bundlePoses.clear();
286  bundleLinks.clear();
287  bundleModels.clear();
288 
289  float maxCorrespondenceDistance = 0.0f;
290  float outlierRatio = 0.0f;
291  if(guess.isNull() &&
292  !regPipeline_->isImageRequired() &&
293  regPipeline_->isScanRequired() &&
294  this->framesProcessed() < 2)
295  {
296  // only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
297  maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
298  outlierRatio = Parameters::defaultIcpOutlierRatio();
299  Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
300  Parameters::parse(parameters_, Parameters::kIcpOutlierRatio(), outlierRatio);
301  ParametersMap params;
302  params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
303  params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(0.95f)));
304  regPipeline_->parseParameters(params);
305  }
306 
307  if(guessIteration == 1)
308  {
309  UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
310  }
311 
312  transform = regPipeline_->computeTransformationMod(
313  tmpMap,
314  *lastFrame_,
315  // special case for ICP-only odom, set guess to identity if we just started or reset
316  guessIteration==0 && !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
317  &regInfo);
318 
319  if(maxCorrespondenceDistance>0.0f)
320  {
321  // set it back
322  ParametersMap params;
323  params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
324  params.insert(ParametersPair(Parameters::kIcpOutlierRatio(), uNumber2Str(outlierRatio)));
325  regPipeline_->parseParameters(params);
326  }
327 
328  data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
329  data.setLaserScan(lastFrame_->sensorData().laserScanRaw());
330 
331  UDEBUG("Registration time = %fs", regInfo.totalTime);
332  if(!transform.isNull())
333  {
334  // local bundle adjustment
335  if(bundleAdjustment_>0 && sba_ &&
336  regPipeline_->isImageRequired() &&
337  !lastFrameModels.empty() &&
338  regInfo.inliersIDs.size())
339  {
340  UDEBUG("Local Bundle Adjustment");
341 
342  // make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
343  UASSERT(map_->getWords().size() && tmpMap.getWords().size());
344  if(map_->getWords().size() != tmpMap.getWords().size() ||
345  map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
346  map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
347  {
348  UERROR("Bundle Adjustment cannot be used with a registration approach recomputing "
349  "features from the \"from\" signature (e.g., Optical Flow) that would change "
350  "their ids (size=old=%ld new=%ld first/last: old=%d->%d new=%d->%d).",
351  map_->getWords().size(), tmpMap.getWords().size(),
352  map_->getWords().begin()->first, map_->getWords().rbegin()->first,
353  tmpMap.getWords().begin()->first, tmpMap.getWords().rbegin()->first);
354  bundleAdjustment_ = 0;
355  }
356  else
357  {
358  UASSERT(bundlePoses_.size());
359  UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
360  UASSERT(bundlePoses_.size() == bundleModels_.size());
361 
362  bundlePoses = bundlePoses_;
363  bundleLinks = bundleLinks_;
364  bundleModels = bundleModels_;
365  bundleLinks.insert(bundleIMUOrientations_.begin(), bundleIMUOrientations_.end());
366 
367  UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
368  uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
369  bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.covariance.inv())));
370  bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
371 
372  if(!imuT.isNull())
373  {
374  bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
375  }
376 
377  bundleModels.insert(std::make_pair(lastFrame_->id(), lastFrameModels));
378 
379  UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
380  std::map<int, std::map<int, FeatureBA> > wordReferences;
381  for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
382  {
383  int wordId =regInfo.inliersIDs[i];
384 
385  // 3D point
386  std::multimap<int, int>::const_iterator iter3D = tmpMap.getWords().find(wordId);
387  UASSERT(iter3D!=tmpMap.getWords().end() && !tmpMap.getWords3().empty());
388  points3DMap.insert(std::make_pair(wordId, tmpMap.getWords3()[iter3D->second]));
389 
390  // all other references
391  std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
392  UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
393 
394  std::map<int, FeatureBA> references;
395  int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
396  if(step == 0)
397  {
398  step = 1;
399  }
400  int oi=0;
401  for(std::map<int, FeatureBA>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
402  {
403  if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
404  {
405  references.insert(*jter);
406  ++totalBundleWordReferencesUsed;
407  }
408  }
409  //make sure the last reference is here
410  if(refIter->second.size() > 1)
411  {
412  if(references.insert(*refIter->second.rbegin()).second)
413  {
414  ++totalBundleWordReferencesUsed;
415  }
416  }
417 
418  std::multimap<int, int>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
419  if(iter2D!=lastFrame_->getWords().end())
420  {
421  UASSERT(!lastFrame_->getWordsKpts().empty());
422  cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter2D->second];
423 
424  int cameraIndex = 0;
425  if(lastFrameModels.size()>1)
426  {
427  UASSERT(lastFrameModels[0].imageWidth()>0);
428  float subImageWidth = lastFrameModels[0].imageWidth();
429  cameraIndex = int(kpt.pt.x / subImageWidth);
430  UASSERT(cameraIndex < (int)lastFrameModels.size());
431  kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
432  }
433 
434  //get depth
435  float d = 0.0f;
436  if( !lastFrame_->getWords3().empty() &&
437  util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
438  {
439  //move back point in camera frame (to get depth along z)
440  d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], lastFrameModels[cameraIndex].localTransform().inverse()).z;
441  }
442  references.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
443  }
444  wordReferences.insert(std::make_pair(wordId, references));
445 
446  //UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
447  //for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
448  //{
449  // UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
450  //}
451  }
452 
453  UDEBUG("sba...start");
454  // set root negative to fix all other poses
455  std::set<int> sbaOutliers;
456  UTimer bundleTimer;
457  bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
458  bundleTime = bundleTimer.ticks();
459  UDEBUG("sba...end");
460  totalBundleOutliers = (int)sbaOutliers.size();
461 
462  UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
463  if(info)
464  {
465  info->localBundlePoses = bundlePoses;
466  info->localBundleModels = bundleModels;
467  }
468 
469  UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
470  if(bundlePoses.size() == bundlePoses_.size()+1)
471  {
472  if(!bundlePoses.rbegin()->second.isNull())
473  {
474  if(sbaOutliers.size())
475  {
476  std::vector<int> newInliers(regInfo.inliersIDs.size());
477  int oi=0;
478  for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
479  {
480  if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
481  {
482  newInliers[oi++] = regInfo.inliersIDs[i];
483  }
484  }
485  newInliers.resize(oi);
486  UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
487  regInfo.inliers = (int)newInliers.size();
488  regInfo.inliersIDs = newInliers;
489  }
490  if(regInfo.inliers < regPipeline_->getMinVisualCorrespondences())
491  {
492  regInfo.rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", regInfo.inliers, regPipeline_->getMinVisualCorrespondences());
493  transform.setNull();
494  }
495  else
496  {
497  transform = bundlePoses.rbegin()->second;
498  std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
499  UASSERT(iter != bundleLinks.end());
500  iter->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
501 
502  iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
503  if(info && iter!=bundleLinks.end() && iter->second.type() == Link::kGravity)
504  {
505  float rollImu,pitchImu,yaw;
506  iter->second.transform().getEulerAngles(rollImu, pitchImu, yaw);
507  float roll,pitch;
508  transform.getEulerAngles(roll, pitch, yaw);
509  info->gravityRollError = fabs(rollImu - roll);
510  info->gravityPitchError = fabs(pitchImu - pitch);
511  }
512 
513  // With bundle adjustment, scale down covariance by 10
514  UASSERT(regInfo.covariance.cols==6 && regInfo.covariance.rows == 6 && regInfo.covariance.type() == CV_64FC1);
515  double thrLin = Registration::COVARIANCE_LINEAR_EPSILON*10.0;
516  double thrAng = Registration::COVARIANCE_ANGULAR_EPSILON*10.0;
517  if(regInfo.covariance.at<double>(0,0)>thrLin)
518  regInfo.covariance.at<double>(0,0) *= 0.1;
519  if(regInfo.covariance.at<double>(1,1)>thrLin)
520  regInfo.covariance.at<double>(1,1) *= 0.1;
521  if(regInfo.covariance.at<double>(2,2)>thrLin)
522  regInfo.covariance.at<double>(2,2) *= 0.1;
523  if(regInfo.covariance.at<double>(3,3)>thrAng)
524  regInfo.covariance.at<double>(3,3) *= 0.1;
525  if(regInfo.covariance.at<double>(4,4)>thrAng)
526  regInfo.covariance.at<double>(4,4) *= 0.1;
527  if(regInfo.covariance.at<double>(5,5)>thrAng)
528  regInfo.covariance.at<double>(5,5) *= 0.1;
529  }
530  }
531  UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
532  }
533  else
534  {
535  UWARN("Local bundle adjustment failed! transform is not refined.");
536  }
537  }
538  }
539 
540  if(!transform.isNull())
541  {
542  // make it incremental
543  transform = this->getPose().inverse() * transform;
544  }
545  }
546 
547  if(transform.isNull())
548  {
549  if(guessIteration == 1)
550  {
551  UWARN("Trial with no guess still fail.");
552  }
553  if(!regInfo.rejectedMsg.empty())
554  {
555  if(guess.isNull())
556  {
557  UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
558  }
559  else
560  {
561  UWARN("Registration failed: \"%s\" (guess=%s)", regInfo.rejectedMsg.c_str(), guess.prettyPrint().c_str());
562  }
563  }
564  else
565  {
566  UWARN("Unknown registration error");
567  }
568  }
569  else if(guessIteration == 1)
570  {
571  UWARN("Trial with no guess succeeded!");
572  }
573  }
574 
575  if(!transform.isNull())
576  {
577  output = transform;
578 
579  bool modified = false;
580  Transform newFramePose = this->getPose()*output;
581 
582  // fields to update
583  LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
584  std::multimap<int, int> mapWords = tmpMap.getWords();
585  std::vector<cv::KeyPoint> mapWordsKpts = tmpMap.getWordsKpts();
586  std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
587  cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
588 
589  bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
590  (keyFrameThr_ == 0.0f ||
591  visKeyFrameThr_ == 0 ||
592  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
593  regInfo.inliers <= visKeyFrameThr_);
594 
595  bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
596  (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
597 
598  addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
599  addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
600 
601  UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
602  if(addKeyFrame)
603  {
604  //Visual
605  int added = 0;
606  int removed = 0;
607  UTimer tmpTimer;
608 
609  UDEBUG("Update local map");
610 
611  // update local map
612  UASSERT(mapWords.size() == mapPoints.size());
613  UASSERT(mapWords.size() == mapWordsKpts.size());
614  UASSERT((int)mapPoints.size() == mapDescriptors.rows);
615  UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
616 
617  std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
618  if(bundleAdjustment_>0)
619  {
620  bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
621  std::multimap<int, Link>::iterator iter = graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false);
622  UASSERT(iter != bundleLinks.end());
623  bundleLinks_.insert(*iter);
624  iter = graph::findLink(bundleLinks, lastFrame_->id(), lastFrame_->id(), false);
625  if(iter != bundleLinks.end())
626  {
627  bundleIMUOrientations_.insert(*iter);
628  }
629  uInsert(bundlePoses_, bundlePoses);
630  UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
631  bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
632  iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
633 
634  // update local map 3D points (if bundle adjustment was done)
635  for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
636  {
637  UASSERT(mapWords.count(iter->first) == 1);
638  //UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
639  mapPoints[mapWords.find(iter->first)->second] = iter->second;
640  }
641  }
642 
643  // sort by feature response
644  std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > > newIds;
645  UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
646  UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
647  std::set<int> seenStatusUpdated;
648 
649  // add points without depth only if the local map has reached its maximum size
650  bool addPointsWithoutDepth = false;
651  if(!visDepthAsMask && validDepthRatio_ < 1.0f && !lastFrame_->getWords3().empty())
652  {
653  int ptsWithDepth = 0;
654  for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
655  iter != lastFrame_->getWords3().end();
656  ++iter)
657  {
658  if(util3d::isFinite(*iter))
659  {
660  ++ptsWithDepth;
661  }
662  }
663  float r = float(ptsWithDepth) / float(lastFrame_->getWords3().size());
664  addPointsWithoutDepth = r > validDepthRatio_;
665  if(!addPointsWithoutDepth)
666  {
667  UWARN("Not enough points with valid depth in current frame (%d/%d=%f < %s=%f), points without depth are not added to map.",
668  ptsWithDepth, (int)lastFrame_->getWords3().size(), r, Parameters::kOdomF2MValidDepthRatio().c_str(), validDepthRatio_);
669  }
670  }
671 
672  if(!lastFrameModels.empty())
673  {
674  for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
675  {
676  const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
677  cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter->second];
678 
679  int cameraIndex = 0;
680  if(lastFrameModels.size()>1)
681  {
682  UASSERT(lastFrameModels[0].imageWidth()>0);
683  float subImageWidth = lastFrameModels[0].imageWidth();
684  cameraIndex = int(kpt.pt.x / subImageWidth);
685  UASSERT(cameraIndex < (int)lastFrameModels.size());
686  kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
687  }
688 
689  if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
690  {
691  if(util3d::isFinite(pt) || addPointsWithoutDepth)
692  {
693  newIds.insert(
694  std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
695  std::make_pair(iter->first,
696  std::make_pair(kpt,
697  std::make_pair(pt,
698  std::make_pair(lastFrame_->getWordsDescriptors().row(iter->second), cameraIndex))))));
699  }
700  }
701  else if(bundleAdjustment_>0)
702  {
703  if(lastFrame_->getWords().count(iter->first) == 1)
704  {
705  std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
706  if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
707  {
708  mapWordsKpts[iterKpts->second].octave = kpt.octave;
709  }
710 
711  UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
712  iterBundlePosesRef->second += 1;
713 
714  //move back point in camera frame (to get depth along z)
715  float depth = 0.0f;
716  if(util3d::isFinite(pt))
717  {
718  depth = util3d::transformPoint(pt, lastFrameModels[cameraIndex].localTransform().inverse()).z;
719  }
720  if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
721  {
722  std::map<int, FeatureBA> framePt;
723  framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
724  bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
725  }
726  else
727  {
728  bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
729  }
730  }
731  }
732  }
733  UDEBUG("newIds=%d", (int)newIds.size());
734  }
735 
736  int lastFrameOldestNewId = lastFrameOldestNewId_;
737  lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
738  for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > >::reverse_iterator iter=newIds.rbegin();
739  iter!=newIds.rend();
740  ++iter)
741  {
742  if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
743  {
744  int cameraIndex = iter->second.second.second.second.second;
745  if(bundleAdjustment_>0)
746  {
747  if(lastFrame_->getWords().count(iter->second.first) == 1)
748  {
749  UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
750  iterBundlePosesRef->second += 1;
751 
752  //move back point in camera frame (to get depth along z)
753  float depth = 0.0f;
754  if(util3d::isFinite(iter->second.second.second.first))
755  {
756  depth = util3d::transformPoint(iter->second.second.second.first, lastFrameModels[cameraIndex].localTransform().inverse()).z;
757  }
758  if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
759  {
760  std::map<int, FeatureBA> framePt;
761  framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
762  bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
763  }
764  else
765  {
766  bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
767  }
768  }
769  }
770 
771  mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
772  mapWordsKpts.push_back(iter->second.second.first);
773  cv::Point3f pt = iter->second.second.second.first;
774  if(!util3d::isFinite(pt))
775  {
776  // get the ray instead
777  float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
778  float y = iter->second.second.first.pt.y;
779  Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
780  lastFrameModels[cameraIndex].imageSize(),
781  x,
782  y,
783  lastFrameModels[cameraIndex].cx(),
784  lastFrameModels[cameraIndex].cy(),
785  lastFrameModels[cameraIndex].fx(),
786  lastFrameModels[cameraIndex].fy());
787  float scaleInf = (0.05 * lastFrameModels[cameraIndex].fx()) / 0.01;
788  pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
789  }
790  mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
791  mapDescriptors.push_back(iter->second.second.second.second.first);
792  if(lastFrameOldestNewId_ > iter->second.first)
793  {
794  lastFrameOldestNewId_ = iter->second.first;
795  }
796  ++added;
797  }
798  }
799  UDEBUG("");
800 
801  // remove words in map if max size is reached
802  if((int)mapWords.size() > maximumMapSize_)
803  {
804  // remove oldest outliers first
805  std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
806  std::vector<int> ids = regInfo.matchesIDs;
807  if(regInfo.projectedIDs.size())
808  {
809  ids.resize(ids.size() + regInfo.projectedIDs.size());
810  int oi=0;
811  for(unsigned int i=0; i<regInfo.projectedIDs.size(); ++i)
812  {
813  if(regInfo.projectedIDs[i]>=lastFrameOldestNewId)
814  {
815  ids[regInfo.matchesIDs.size()+oi++] = regInfo.projectedIDs[i];
816  }
817  }
818  ids.resize(regInfo.matchesIDs.size()+oi);
819  UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
820  }
821  for(unsigned int i=0; i<ids.size() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size(); ++i)
822  {
823  int id = ids.at(i);
824  if(inliers.find(id) == inliers.end())
825  {
826  std::map<int, std::map<int, FeatureBA> >::iterator iterRef = bundleWordReferences_.find(id);
827  if(iterRef != bundleWordReferences_.end())
828  {
829  for(std::map<int, FeatureBA>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
830  {
831  if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
832  {
833  bundlePoseReferences_.at(iterFrame->first) -= 1;
834  }
835  }
836  bundleWordReferences_.erase(iterRef);
837  }
838 
839  mapWords.erase(id);
840  ++removed;
841  }
842  }
843 
844  // remove oldest first
845  for(std::multimap<int, int>::iterator iter = mapWords.begin();
846  iter!=mapWords.end() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size();)
847  {
848  if(inliers.find(iter->first) == inliers.end())
849  {
850  std::map<int, std::map<int, FeatureBA> >::iterator iterRef = bundleWordReferences_.find(iter->first);
851  if(iterRef != bundleWordReferences_.end())
852  {
853  for(std::map<int, FeatureBA>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
854  {
855  if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
856  {
857  bundlePoseReferences_.at(iterFrame->first) -= 1;
858  }
859  }
860  bundleWordReferences_.erase(iterRef);
861  }
862 
863  mapWords.erase(iter++);
864  ++removed;
865  }
866  else
867  {
868  ++iter;
869  }
870  }
871 
872  if(mapWords.size() != mapPoints.size())
873  {
874  UDEBUG("Remove points");
875  std::vector<cv::KeyPoint> mapWordsKptsClean(mapWords.size());
876  std::vector<cv::Point3f> mapPointsClean(mapWords.size());
877  cv::Mat mapDescriptorsClean(mapWords.size(), mapDescriptors.cols, mapDescriptors.type());
878  int index = 0;
879  for(std::multimap<int, int>::iterator iter = mapWords.begin(); iter!=mapWords.end(); ++iter, ++index)
880  {
881  mapWordsKptsClean[index] = mapWordsKpts[iter->second];
882  mapPointsClean[index] = mapPoints[iter->second];
883  mapDescriptors.row(iter->second).copyTo(mapDescriptorsClean.row(index));
884  iter->second = index;
885  }
886  mapWordsKpts = mapWordsKptsClean;
887  mapWordsKptsClean.clear();
888  mapPoints = mapPointsClean;
889  mapPointsClean.clear();
890  mapDescriptors = mapDescriptorsClean;
891  }
892 
893  Link * previousLink = 0;
894  for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
895  {
896  if(iter->second <= 0)
897  {
898  if(previousLink == 0 || bundleLinks_.find(iter->first) != bundleLinks_.end())
899  {
900  if(previousLink)
901  {
902  UASSERT(previousLink->to() == iter->first);
903  *previousLink = previousLink->merge(bundleLinks_.find(iter->first)->second, previousLink->type());
904  }
905  UASSERT(bundlePoses_.erase(iter->first) == 1);
906  bundleLinks_.erase(iter->first);
907  bundleModels_.erase(iter->first);
908  bundleIMUOrientations_.erase(iter->first);
909  bundlePoseReferences_.erase(iter++);
910  }
911  }
912  else
913  {
914  previousLink=0;
915  if(bundleLinks_.find(iter->first) != bundleLinks_.end())
916  {
917  previousLink = &bundleLinks_.find(iter->first)->second;
918  }
919  ++iter;
920  }
921  }
922  }
923 
924  if(added || removed)
925  {
926  modified = true;
927  }
928  UDEBUG("Update local features map = %fs", tmpTimer.ticks());
929 
930  // Geometric
931  UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
932  UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
933 
934  if(lastFrame_->sensorData().laserScanRaw().size())
935  {
936  pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
937  Transform viewpoint = newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform();
938  pcl::PointCloud<pcl::PointXYZINormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointXYZINormal>());
939 
940  if(scanMapMaxRange_ > 0)
941  {
942  frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw());
943  frameCloudNormals = util3d::cropBox(frameCloudNormals,
944  Eigen::Vector4f(-scanMapMaxRange_ / 2, -scanMapMaxRange_ / 2,-scanMapMaxRange_ / 2, 0),
945  Eigen::Vector4f(scanMapMaxRange_ / 2,scanMapMaxRange_ / 2,scanMapMaxRange_ / 2, 0)
946  );
947  frameCloudNormals = util3d::transformPointCloud(frameCloudNormals, viewpoint);
948  } else
949  {
950  frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), viewpoint);
951  }
952 
953  pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
954  int newPoints;
955  if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
956  {
957  // remove points that overlap (the ones found in both clouds)
958  frameCloudNormalsIndices = util3d::subtractFiltering(
959  frameCloudNormals,
960  pcl::IndicesPtr(new std::vector<int>),
961  mapCloudNormals,
962  pcl::IndicesPtr(new std::vector<int>),
963  scanSubtractRadius_,
964  lastFrame_->sensorData().laserScanRaw().hasNormals()&&mapScan.hasNormals()?scanSubtractAngle_:0.0f);
965  newPoints = frameCloudNormalsIndices->size();
966  }
967  else
968  {
969  newPoints = frameCloudNormals->size();
970  }
971 
972  if(newPoints)
973  {
974  if (scanMapMaxRange_ > 0) {
975  // Copying new points to tmp cloud
976  // These are the points that have no overlap between mapScan and lastFrame
977  pcl::PointCloud<pcl::PointXYZINormal> tmp;
978  pcl::copyPointCloud(*frameCloudNormals, *frameCloudNormalsIndices, tmp);
979 
980  if (int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) // 20 000 points
981  {
982  // Print mapSize
983  UINFO("mapSize=%d newPoints=%d maxPoints=%d",
984  int(mapCloudNormals->size()),
985  newPoints,
986  scanMaximumMapSize_);
987 
988  *mapCloudNormals += tmp;
989  cv::Point3f boxMin (-scanMapMaxRange_/2, -scanMapMaxRange_/2, -scanMapMaxRange_/2);
990  cv::Point3f boxMax (scanMapMaxRange_/2, scanMapMaxRange_/2, scanMapMaxRange_/2);
991 
992  boxMin = util3d::transformPoint(boxMin, viewpoint.translation());
993  boxMax = util3d::transformPoint(boxMax, viewpoint.translation());
994 
995  mapCloudNormals = util3d::cropBox(mapCloudNormals, Eigen::Vector4f(boxMin.x, boxMin.y, boxMin.z, 0 ), Eigen::Vector4f(boxMax.x, boxMax.y, boxMax.z, 0 ));
996 
997  } else {
998  *mapCloudNormals += tmp;
999  }
1000 
1001  mapCloudNormals = util3d::voxelize(mapCloudNormals, scanSubtractRadius_);
1002  pcl::PointCloud<pcl::PointXYZI>::Ptr mapCloud (new pcl::PointCloud<pcl::PointXYZI> ());
1003  copyPointCloud(*mapCloudNormals, *mapCloud);
1004  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(mapCloud, pointToPlaneK_, pointToPlaneRadius_, Eigen::Vector3f(viewpoint.x(), viewpoint.y(), viewpoint.z()));
1005  copyPointCloud(*normals, *mapCloudNormals);
1006 
1007  } else {
1008  scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
1009 
1010  //remove points if too big
1011  UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
1012  (int)scansBuffer_.size(),
1013  int(mapCloudNormals->size()),
1014  newPoints,
1015  scanMaximumMapSize_);
1016 
1017  if(scansBuffer_.size() > 1 &&
1018  int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
1019  {
1020  //regenerate the local map
1021  mapCloudNormals->clear();
1022  std::list<int> toRemove;
1023  int i = int(scansBuffer_.size())-1;
1024  for(; i>=0; --i)
1025  {
1026  int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
1027  if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
1028  i == 0)
1029  {
1030  *mapCloudNormals += *scansBuffer_[i].first;
1031  break;
1032  }
1033  else
1034  {
1035  if(scansBuffer_[i].second->size())
1036  {
1037  pcl::PointCloud<pcl::PointXYZINormal> tmp;
1038  pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
1039  *mapCloudNormals += tmp;
1040  }
1041  else
1042  {
1043  *mapCloudNormals += *scansBuffer_[i].first;
1044  }
1045  }
1046  }
1047  // remove old clouds
1048  if(i > 0)
1049  {
1050  std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
1051  int oi = 0;
1052  for(; i<(int)scansBuffer_.size(); ++i)
1053  {
1054  UASSERT(oi < (int)scansTmp.size());
1055  scansTmp[oi++] = scansBuffer_[i];
1056  }
1057  scansBuffer_ = scansTmp;
1058  }
1059  }
1060  else
1061  {
1062  // just append the last cloud
1063  if(scansBuffer_.back().second->size())
1064  {
1065  pcl::PointCloud<pcl::PointXYZINormal> tmp;
1066  pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
1067  *mapCloudNormals += tmp;
1068  }
1069  else
1070  {
1071  *mapCloudNormals += *scansBuffer_.back().first;
1072  }
1073  }
1074  }
1075 
1076  if(mapScan.is2d())
1077  {
1078  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
1079  mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f);
1080  }
1081  else
1082  {
1083  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
1084  mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f);
1085  }
1086  modified=true;
1087  }
1088  }
1089  UDEBUG("Update local scan map = %fs", tmpTimer.ticks());
1090  }
1091 
1092  if(modified)
1093  {
1094  *map_ = tmpMap;
1095 
1096  if(mapScan.is2d())
1097  {
1098 
1099  map_->sensorData().setLaserScan(
1100  LaserScan(
1101  mapScan.data(),
1102  0,
1103  0.0f,
1104  mapScan.format(),
1105  Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
1106  }
1107  else
1108  {
1109  map_->sensorData().setLaserScan(
1110  LaserScan(
1111  mapScan.data(),
1112  0,
1113  0.0f,
1114  mapScan.format(),
1115  newFramePose.translation()));
1116  }
1117 
1118  map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
1119  }
1120  }
1121 
1122  if(info)
1123  {
1124  // use tmpMap instead of map_ to make sure that correspondences with the new frame matches
1125  info->localMapSize = (int)tmpMap.getWords3().size();
1126  info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
1127  if(this->isInfoDataFilled())
1128  {
1129  info->localMap.clear();
1130  if(!tmpMap.getWords3().empty())
1131  {
1132  for(std::multimap<int, int>::const_iterator iter=tmpMap.getWords().begin(); iter!=tmpMap.getWords().end(); ++iter)
1133  {
1134  info->localMap.insert(std::make_pair(iter->first, tmpMap.getWords3()[iter->second]));
1135  }
1136  }
1137  info->localScanMap = tmpMap.sensorData().laserScanRaw();
1138  }
1139  }
1140  }
1141  else
1142  {
1143  // Just generate keypoints for the new signature
1144  // For scan, we want to use reading filters, so set dummy's scan and set back to reference afterwards
1145  Signature dummy;
1146  dummy.sensorData().setLaserScan(lastFrame_->sensorData().laserScanRaw());
1147  lastFrame_->sensorData().setLaserScan(LaserScan());
1148  regPipeline_->computeTransformationMod(
1149  *lastFrame_,
1150  dummy);
1151  lastFrame_->sensorData().setLaserScan(dummy.sensorData().laserScanRaw());
1152 
1153  data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
1154  data.setLaserScan(lastFrame_->sensorData().laserScanRaw());
1155 
1156  // a very high variance tells that the new pose is not linked with the previous one
1157  regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
1158 
1159  bool frameValid = false;
1160  Transform newFramePose = this->getPose(); // initial pose may be not identity...
1161 
1162  if(regPipeline_->isImageRequired())
1163  {
1164  int ptsWithDepth = 0;
1165  for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
1166  iter != lastFrame_->getWords().end();
1167  ++iter)
1168  {
1169  if(!lastFrame_->getWords3().empty() &&
1170  util3d::isFinite(lastFrame_->getWords3()[iter->second]))
1171  {
1172  ++ptsWithDepth;
1173  }
1174  }
1175 
1176  if (ptsWithDepth >= regPipeline_->getMinVisualCorrespondences())
1177  {
1178  frameValid = true;
1179  // update local map
1180  UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
1181  UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
1182 
1183  std::multimap<int, int> words;
1184  std::vector<cv::KeyPoint> wordsKpts;
1185  std::vector<cv::Point3f> transformedPoints;
1186  std::multimap<int, int> mapPointWeights;
1187  cv::Mat descriptors;
1188  if(!lastFrame_->getWords3().empty() && !lastFrameModels.empty())
1189  {
1190  for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
1191  iter != lastFrame_->getWords().end();
1192  ++iter)
1193  {
1194  const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
1195  if (util3d::isFinite(pt))
1196  {
1197  words.insert(words.end(), std::make_pair(iter->first, words.size()));
1198  wordsKpts.push_back(lastFrame_->getWordsKpts()[iter->second]);
1199  transformedPoints.push_back(util3d::transformPoint(pt, newFramePose));
1200  mapPointWeights.insert(std::make_pair(iter->first, 0));
1201  descriptors.push_back(lastFrame_->getWordsDescriptors().row(iter->second));
1202  }
1203  }
1204  }
1205 
1206  if(bundleAdjustment_>0)
1207  {
1208  // update bundleWordReferences_: used for bundle adjustment
1209  if(!wordsKpts.empty())
1210  {
1211  for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
1212  {
1213  if(words.count(iter->first) == 1)
1214  {
1215  UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
1216  std::map<int, FeatureBA> framePt;
1217 
1218  cv::KeyPoint kpt = wordsKpts[iter->second];
1219 
1220  int cameraIndex = 0;
1221  if(lastFrameModels.size()>1)
1222  {
1223  UASSERT(lastFrameModels[0].imageWidth()>0);
1224  float subImageWidth = lastFrameModels[0].imageWidth();
1225  cameraIndex = int(kpt.pt.x / subImageWidth);
1226  kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
1227  }
1228 
1229  //get depth
1230  float d = 0.0f;
1231  if(lastFrame_->getWords().count(iter->first) == 1 &&
1232  !lastFrame_->getWords3().empty() &&
1233  util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
1234  {
1235  //move back point in camera frame (to get depth along z)
1236  d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], lastFrameModels[cameraIndex].localTransform().inverse()).z;
1237  }
1238 
1239 
1240  framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
1241  bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
1242  }
1243  }
1244  }
1245 
1246  bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
1247  bundleModels_.insert(std::make_pair(lastFrame_->id(), lastFrameModels));
1248  bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
1249 
1250  if(!imuT.isNull())
1251  {
1252  bundleIMUOrientations_.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, newFramePose)));
1253  }
1254  }
1255 
1256  map_->setWords(words, wordsKpts, transformedPoints, descriptors);
1257  addKeyFrame = true;
1258  }
1259  else
1260  {
1261  UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
1262  }
1263  }
1264  if(regPipeline_->isScanRequired())
1265  {
1266  if (lastFrame_->sensorData().laserScanRaw().size())
1267  {
1268  pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
1269 
1270  double complexity = 0.0;;
1271  if(!frameValid)
1272  {
1273  float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
1274  bool p2n = Parameters::defaultIcpPointToPlane();
1275  Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
1276  Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
1277  if(p2n && minComplexity>0.0f)
1278  {
1279  if(lastFrame_->sensorData().laserScanRaw().hasNormals())
1280  {
1281  complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
1282  if(complexity > minComplexity)
1283  {
1284  frameValid = true;
1285  }
1286  else if(!guess.isNull() && !guess.isIdentity())
1287  {
1288  UWARN("Scan complexity too low (%f) to init robustly the first "
1289  "keyframe. Make sure the lidar is seeing enough "
1290  "geometry in all axes for good initialization. "
1291  "Accepting as an initial guess (%s) is provided.",
1292  complexity,
1293  guess.prettyPrint().c_str());
1294  frameValid = true;
1295  }
1296  }
1297  else
1298  {
1299  UWARN("Input raw scan doesn't have normals, complexity check on first frame is not done.");
1300  frameValid = true;
1301  }
1302  }
1303  else
1304  {
1305  frameValid = true;
1306  }
1307  }
1308 
1309  if(frameValid)
1310  {
1311  if (scanMapMaxRange_ > 0 ){
1312  UINFO("Local map will be updated using range instead of time with range threshold set at %f", scanMapMaxRange_);
1313  } else {
1314  scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
1315  }
1316  if(lastFrame_->sensorData().laserScanRaw().is2d())
1317  {
1318  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
1319  map_->sensorData().setLaserScan(
1320  LaserScan(
1321  util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
1322  0,
1323  0.0f,
1324  Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
1325  }
1326  else
1327  {
1328  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
1329  map_->sensorData().setLaserScan(
1330  LaserScan(
1331  util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
1332  0,
1333  0.0f,
1334  newFramePose.translation()));
1335  }
1336 
1337  addKeyFrame = true;
1338  }
1339  else
1340  {
1341  UWARN("Scan complexity too low (%f) to init first keyframe.", complexity);
1342  }
1343  }
1344  else
1345  {
1346  UWARN("Missing scan to initialize odometry.");
1347  }
1348  }
1349 
1350  if (frameValid)
1351  {
1352  // We initialized the local map
1353  output.setIdentity();
1354  }
1355 
1356  if(info)
1357  {
1358  info->localMapSize = (int)map_->getWords3().size();
1359  info->localScanMapSize = map_->sensorData().laserScanRaw().size();
1360 
1361  if(this->isInfoDataFilled())
1362  {
1363  info->localMap.clear();
1364  if(!map_->getWords3().empty())
1365  {
1366  for(std::multimap<int, int>::const_iterator iter=map_->getWords().begin(); iter!=map_->getWords().end(); ++iter)
1367  {
1368  info->localMap.insert(std::make_pair(iter->first, map_->getWords3()[iter->second]));
1369  }
1370  }
1371  info->localScanMap = map_->sensorData().laserScanRaw();
1372  }
1373  }
1374  }
1375 
1376  map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat()); // clear sensorData features
1377 
1378  nFeatures = lastFrame_->getWords().size();
1379  if(this->isInfoDataFilled() && info)
1380  {
1381  if(regPipeline_->isImageRequired())
1382  {
1383  info->words.clear();
1384  if(!lastFrame_->getWordsKpts().empty())
1385  {
1386  for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
1387  {
1388  info->words.insert(std::make_pair(iter->first, lastFrame_->getWordsKpts()[iter->second]));
1389  }
1390  }
1391  }
1392  }
1393  }
1394  else
1395  {
1396  UERROR("SensorData not valid!");
1397  }
1398 
1399  if(info)
1400  {
1401  info->features = nFeatures;
1402  info->localKeyFrames = (int)bundlePoses_.size();
1403  info->keyFrameAdded = addKeyFrame;
1404  info->localBundleOutliers = totalBundleOutliers;
1405  info->localBundleConstraints = totalBundleWordReferencesUsed;
1406  info->localBundleTime = bundleTime;
1407 
1408  if(this->isInfoDataFilled())
1409  {
1410  info->reg = regInfo;
1411  }
1412  else
1413  {
1414  info->reg = regInfo.copyWithoutData();
1415  }
1416  }
1417 
1418  UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
1419  timer.elapsed(),
1420  output.isNull()?"true":"false",
1421  nFeatures,
1422  regInfo.inliers,
1423  regInfo.matches,
1424  !regInfo.covariance.empty()?regInfo.covariance.at<double>(0,0):0,
1425  !regInfo.covariance.empty()?regInfo.covariance.at<double>(5,5):0,
1426  regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
1427  regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().size():0);
1428  return output;
1429 }
1430 
1431 } // namespace rtabmap
d
d
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::Registration::getMinVisualCorrespondences
int getMinVisualCorrespondences() const
Definition: Registration.cpp:130
rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON
static double COVARIANCE_ANGULAR_EPSILON
Definition: Registration.h:49
rtabmap::util3d::subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
Definition: util3d_filtering.cpp:1413
rtabmap::OdometryF2M::parameters_
ParametersMap parameters_
Definition: OdometryF2M.h:90
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
UTimer
Definition: UTimer.h:46
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:992
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:271
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::OdometryF2M::OdometryF2M
OdometryF2M(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryF2M.cpp:57
f
f
x
x
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:203
util3d_motion_estimation.h
rtabmap::Odometry::framesProcessed
unsigned int framesProcessed() const
Definition: Odometry.h:81
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:126
rtabmap::OdometryF2M::regPipeline_
Registration * regPipeline_
Definition: OdometryF2M.h:76
rtabmap::OdometryF2M::scansBuffer_
std::vector< std::pair< pcl::PointCloud< pcl::PointXYZINormal >::Ptr, pcl::IndicesPtr > > scansBuffer_
Definition: OdometryF2M.h:80
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::Signature::getWordsDescriptors
const cv::Mat & getWordsDescriptors() const
Definition: Signature.h:115
rtabmap::OdometryInfo::localBundleModels
std::map< int, std::vector< CameraModel > > localBundleModels
Definition: OdometryInfo.h:106
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
rtabmap::OdometryF2M::bundleModels_
std::map< int, std::vector< CameraModel > > bundleModels_
Definition: OdometryF2M.h:86
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
rtabmap::OdometryF2M::sba_
Optimizer * sba_
Definition: OdometryF2M.h:89
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
uBool2Str
std::string UTILITE_EXP uBool2Str(bool boolean)
Definition: UConversion.cpp:156
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:270
rtabmap::OdometryF2M::computeTransform
virtual Transform computeTransform(SensorData &data, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryF2M.cpp:199
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:131
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UMath.h
Basic mathematics functions.
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:175
UConversion.h
Some conversion functions.
rtabmap::OdometryF2M::bundleWordReferences_
std::map< int, std::map< int, FeatureBA > > bundleWordReferences_
Definition: OdometryF2M.h:82
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
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:437
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
rtabmap::RegistrationInfo::copyWithoutData
RegistrationInfo copyWithoutData() const
Definition: RegistrationInfo.h:55
glm::step
GLM_FUNC_DECL genType step(genType const &edge, genType const &x)
util3d_registration.h
rtabmap::Optimizer::gravitySigma
float gravitySigma() const
Definition: Optimizer.h:98
rtabmap::Registration::COVARIANCE_LINEAR_EPSILON
static double COVARIANCE_LINEAR_EPSILON
Definition: Registration.h:48
rtabmap::OdometryF2M::bundleIMUOrientations_
std::multimap< int, Link > bundleIMUOrientations_
Definition: OdometryF2M.h:85
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:112
UASSERT
#define UASSERT(condition)
rtabmap::OdometryF2M::lastFrame_
Signature * lastFrame_
Definition: OdometryF2M.h:78
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::OdometryF2M::bundlePoseReferences_
std::map< int, int > bundlePoseReferences_
Definition: OdometryF2M.h:87
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:100
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2285
util3d_filtering.h
rtabmap::OdometryF2M::bundlePoses_
std::map< int, Transform > bundlePoses_
Definition: OdometryF2M.h:83
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:79
rtabmap::Transform::getTransform
static Transform getTransform(const std::map< double, Transform > &tfBuffer, const double &stamp)
Definition: Transform.cpp:520
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:272
rtabmap::OdometryInfo::localBundlePoses
std::map< int, Transform > localBundlePoses
Definition: OdometryInfo.h:105
rtabmap::Odometry::imus
const std::map< double, Transform > & imus() const
Definition: Odometry.h:85
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:90
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
util3d_transforms.h
params
params
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:39
rtabmap::OdometryF2M::map_
Signature * map_
Definition: OdometryF2M.h:77
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
UDEBUG
#define UDEBUG(...)
rtabmap::util3d::projectDepthTo3DRay
Eigen::Vector3f RTABMAP_EXP projectDepthTo3DRay(const cv::Size &imageSize, float x, float y, float cx, float cy, float fx, float fy)
Definition: util3d.cpp:245
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:125
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
rtabmap::Signature::setWords
void setWords(const std::multimap< int, int > &words, const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &words3, const cv::Mat &descriptors)
Definition: Signature.cpp:262
UERROR
#define UERROR(...)
rtabmap::util3d::cropBox
pcl::IndicesPtr RTABMAP_EXP cropBox(const pcl::PCLPointCloud2::Ptr &cloud, const pcl::IndicesPtr &indices, const Eigen::Vector4f &min, const Eigen::Vector4f &max, const Transform &transform=Transform::getIdentity(), bool negative=false)
Definition: util3d_filtering.cpp:816
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
trace.model
model
Definition: trace.py:4
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::OdometryF2M::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryF2M.cpp:180
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
rtabmap::OdometryF2M::bundleLinks_
std::multimap< int, Link > bundleLinks_
Definition: OdometryF2M.h:84
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:603
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:156
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::Transform::translation
Transform translation() const
Definition: Transform.cpp:203
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1920
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::util3d::computeNormalsComplexity
float RTABMAP_EXP computeNormalsComplexity(const LaserScan &scan, const Transform &t=Transform::getIdentity(), cv::Mat *pcaEigenVectors=0, cv::Mat *pcaEigenValues=0)
Definition: util3d_surface.cpp:3084
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111
util3d_surface.h
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 Jan 23 2023 03:37:29