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

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