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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:57