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

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