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/VisualWord.h"
31 #include "rtabmap/core/Signature.h"
32 #include "rtabmap/core/RegistrationVis.h"
33 #include "rtabmap/core/util3d_transforms.h"
34 #include "rtabmap/core/util3d_registration.h"
35 #include "rtabmap/core/util3d_correspondences.h"
36 #include "rtabmap/core/util3d_motion_estimation.h"
37 #include "rtabmap/core/util3d_filtering.h"
38 #include "rtabmap/core/Optimizer.h"
39 #include "rtabmap/core/VWDictionary.h"
40 #include "rtabmap/core/util3d.h"
41 #include "rtabmap/core/Graph.h"
42 #include "rtflann/flann.hpp"
43 #include "rtabmap/utilite/ULogger.h"
44 #include "rtabmap/utilite/UTimer.h"
45 #include "rtabmap/utilite/UMath.h"
46 #include "rtabmap/utilite/UConversion.h"
47 #include <opencv2/calib3d/calib3d.hpp>
48 #include <rtabmap/core/OdometryF2M.h>
49 #include <pcl/common/io.h>
50 
51 #if _MSC_VER
52  #define ISFINITE(value) _finite(value)
53 #else
54  #define ISFINITE(value) std::isfinite(value)
55 #endif
56 
57 namespace rtabmap {
58 
59 OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
60  Odometry(parameters),
61  maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
62  keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
63  visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
64  maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
65  scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
66  scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
67  scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
68  scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
69  bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
70  bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
71  map_(new Signature(-1)),
72  lastFrame_(new Signature(1)),
73  lastFrameOldestNewId_(0),
74  bundleSeq_(0),
75  sba_(0)
76 {
77  UDEBUG("");
78  Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
79  Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
80  Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
81  Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
82  Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
83  Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
84  Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
85  if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
86  {
87  scanSubtractAngle_ *= M_PI/180.0f;
88  }
89  Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
90  Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
91  UASSERT(bundleMaxFrames_ >= 0);
92  ParametersMap bundleParameters = parameters;
93  if(bundleAdjustment_ > 0)
94  {
95  if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
96  (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)))
97  {
98  // disable bundle in RegistrationVis as we do it already here
99  uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
100  sba_ = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
101  }
102  else
103  {
104  UWARN("Selected bundle adjustment approach (\"%s\"=\"%d\") is not available, "
105  "local bundle adjustment is then disabled.", Parameters::kOdomF2MBundleAdjustment().c_str(), bundleAdjustment_);
106  bundleAdjustment_ = 0;
107  }
108  }
109  UASSERT(maximumMapSize_ >= 0);
110  UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
111  UASSERT(visKeyFrameThr_>=0);
112  UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
113  UASSERT(maxNewFeatures_ >= 0);
114 
115  int corType = Parameters::defaultVisCorType();
116  Parameters::parse(parameters, Parameters::kVisCorType(), corType);
117  if(corType != 0)
118  {
119  UWARN("%s=%d is not supported by OdometryF2M, using Features matching approach instead (type=0).",
120  Parameters::kVisCorType().c_str(),
121  corType);
122  corType = 0;
123  }
124  uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
125 
126  regPipeline_ = Registration::create(bundleParameters);
127  if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
128  {
129  UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
130  Parameters::kOdomF2MBundleAdjustment().c_str(),
131  bundleAdjustment_,
132  Parameters::kRegStrategy().c_str(),
133  uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
134  bundleAdjustment_ = 0;
135  }
136 
137  parameters_ = bundleParameters;
138 }
139 
140 OdometryF2M::~OdometryF2M()
141 {
142  delete map_;
143  delete lastFrame_;
144  scansBuffer_.clear();
145  bundleWordReferences_.clear();
146  bundlePoses_.clear();
147  bundleLinks_.clear();
148  bundleModels_.clear();
149  bundlePoseReferences_.clear();
150  delete sba_;
151  delete regPipeline_;
152  UDEBUG("");
153 }
154 
155 
156 void OdometryF2M::reset(const Transform & initialPose)
157 {
158  Odometry::reset(initialPose);
159  *lastFrame_ = Signature(1);
160  *map_ = Signature(-1);
161  scansBuffer_.clear();
162  bundleWordReferences_.clear();
163  bundlePoses_.clear();
164  bundleLinks_.clear();
165  bundleModels_.clear();
166  bundlePoseReferences_.clear();
167  bundleSeq_ = 0;
168  lastFrameOldestNewId_ = 0;
169 }
170 
171 // return not null transform if odometry is correctly computed
172 Transform OdometryF2M::computeTransform(
173  SensorData & data,
174  const Transform & guess,
175  OdometryInfo * info)
176 {
177  UTimer timer;
178  Transform output;
179 
180  if(info)
181  {
182  info->type = 0;
183  }
184 
185  RegistrationInfo regInfo;
186  int nFeatures = 0;
187 
188  delete lastFrame_;
189  int id = data.id();
190  data.setId(++bundleSeq_); // generate our own unique ids, to make sure they are correctly set
191  lastFrame_ = new Signature(data);
192  data.setId(id);
193 
194  if(bundleAdjustment_ > 0 &&
195  data.cameraModels().size() > 1)
196  {
197  UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
198  bundleAdjustment_ = 0;
199  }
200  bool addKeyFrame = false;
201  int totalBundleWordReferencesUsed = 0;
202  int totalBundleOutliers = 0;
203  float bundleTime = 0.0f;
204 
205  // Generate keypoints from the new data
206  if(lastFrame_->sensorData().isValid())
207  {
208  if((map_->getWords3().size() || !map_->sensorData().laserScanRaw().isEmpty()) &&
209  lastFrame_->sensorData().isValid())
210  {
211  Signature tmpMap;
212  Transform transform;
213  UDEBUG("guess=%s frames=%d image required=%d", guess.prettyPrint().c_str(), this->framesProcessed(), regPipeline_->isImageRequired()?1:0);
214 
215  // bundle adjustment stuff if used
216  std::map<int, cv::Point3f> points3DMap;
217  std::map<int, Transform> bundlePoses;
218  std::multimap<int, Link> bundleLinks;
219  std::map<int, CameraModel> bundleModels;
220  std::map<int, StereoCameraModel> bundleStereoModels;
221 
222  for(int guessIteration=0;
223  guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
224  ++guessIteration)
225  {
226  tmpMap = *map_;
227  // reset matches, but keep already extracted features in lastFrame_->sensorData()
228  lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
229  lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
230  lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
231 
232  points3DMap.clear();
233  bundlePoses.clear();
234  bundleLinks.clear();
235  bundleModels.clear();
236  bundleStereoModels.clear();
237 
238  float maxCorrespondenceDistance = 0.0f;
239  float pmOutlierRatio = 0.0f;
240  if(guess.isNull() &&
241  !regPipeline_->isImageRequired() &&
242  regPipeline_->isScanRequired() &&
243  this->framesProcessed() < 2)
244  {
245  // only on initialization (first frame to register), increase icp max correspondences in case the robot is already moving
246  maxCorrespondenceDistance = Parameters::defaultIcpMaxCorrespondenceDistance();
247  pmOutlierRatio = Parameters::defaultIcpPMOutlierRatio();
248  Parameters::parse(parameters_, Parameters::kIcpMaxCorrespondenceDistance(), maxCorrespondenceDistance);
249  Parameters::parse(parameters_, Parameters::kIcpPMOutlierRatio(), pmOutlierRatio);
250  ParametersMap params;
251  params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance*3.0f)));
252  params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(0.95f)));
253  regPipeline_->parseParameters(params);
254  }
255 
256  if(guessIteration == 1)
257  {
258  UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
259  }
260 
261  transform = regPipeline_->computeTransformationMod(
262  tmpMap,
263  *lastFrame_,
264  // special case for ICP-only odom, set guess to identity if we just started or reset
265  guessIteration==0 && !guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
266  &regInfo);
267 
268  if(maxCorrespondenceDistance>0.0f)
269  {
270  // set it back
271  ParametersMap params;
272  params.insert(ParametersPair(Parameters::kIcpMaxCorrespondenceDistance(), uNumber2Str(maxCorrespondenceDistance)));
273  params.insert(ParametersPair(Parameters::kIcpPMOutlierRatio(), uNumber2Str(pmOutlierRatio)));
274  regPipeline_->parseParameters(params);
275  }
276 
277  data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
278 
279  UDEBUG("Registration time = %fs", regInfo.totalTime);
280  if(!transform.isNull())
281  {
282  // local bundle adjustment
283  if(bundleAdjustment_>0 && sba_ &&
284  regPipeline_->isImageRequired() &&
285  lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
286  regInfo.inliersIDs.size())
287  {
288  UDEBUG("Local Bundle Adjustment");
289 
290  // make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
291  UASSERT(map_->getWords().size() && tmpMap.getWords().size());
292  if(map_->getWords().size() != tmpMap.getWords().size() ||
293  map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
294  map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
295  {
296  UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
297  bundleAdjustment_ = 0;
298  }
299  else
300  {
301  UASSERT(bundlePoses_.size());
302  UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
303  UASSERT(bundlePoses_.size() == bundleModels_.size());
304 
305  bundlePoses = bundlePoses_;
306  bundleLinks = bundleLinks_;
307  bundleModels = bundleModels_;
308 
309  UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
310  uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
311  cv::Mat var = regInfo.covariance;//cv::Mat::eye(6,6,CV_64FC1); //regInfo.covariance.inv()
312  //var(cv::Range(0,3), cv::Range(0,3)) *= 0.001;
313  //var(cv::Range(3,6), cv::Range(3,6)) *= 0.001;
314  bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, var.inv())));
315  bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
316 
317  CameraModel model;
318  if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
319  {
320  model = lastFrame_->sensorData().cameraModels()[0];
321  }
322  else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
323  {
324  model = lastFrame_->sensorData().stereoCameraModel().left();
325  // Set Tx for stereo BA
326  model = CameraModel(model.fx(),
327  model.fy(),
328  model.cx(),
329  model.cy(),
330  model.localTransform(),
331  -lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
332  }
333  else
334  {
335  UFATAL("no valid camera model to do odometry bundle adjustment!");
336  }
337  bundleModels.insert(std::make_pair(lastFrame_->id(), model));
338  Transform invLocalTransform = model.localTransform().inverse();
339 
340  UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
341  std::map<int, std::map<int, cv::Point3f> > wordReferences;
342  for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
343  {
344  int wordId =regInfo.inliersIDs[i];
345 
346  // 3D point
347  std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
348  UASSERT(iter3D!=tmpMap.getWords3().end());
349  points3DMap.insert(*iter3D);
350 
351  std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
352 
353  // all other references
354  std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
355  UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
356 
357  std::map<int, cv::Point3f> references;
358  int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
359  if(step == 0)
360  {
361  step = 1;
362  }
363  int oi=0;
364  for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
365  {
366  if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
367  {
368  references.insert(*jter);
369  ++totalBundleWordReferencesUsed;
370  }
371  }
372  //make sure the last reference is here
373  if(refIter->second.size() > 1)
374  {
375  if(references.insert(*refIter->second.rbegin()).second)
376  {
377  ++totalBundleWordReferencesUsed;
378  }
379  }
380 
381  if(iter2D!=lastFrame_->getWords().end())
382  {
383  UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
384  //move back point in camera frame (to get depth along z)
385  cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
386  references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
387  }
388  wordReferences.insert(std::make_pair(wordId, references));
389 
390  //UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
391  //for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
392  //{
393  // UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
394  //}
395  }
396 
397  UDEBUG("sba...start");
398  // set root negative to fix all other poses
399  std::set<int> sbaOutliers;
400  UTimer bundleTimer;
401  bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
402  bundleTime = bundleTimer.ticks();
403  UDEBUG("sba...end");
404  totalBundleOutliers = (int)sbaOutliers.size();
405 
406  UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.size());
407  if(info)
408  {
409  info->localBundlePoses = bundlePoses;
410  info->localBundleModels = bundleModels;
411  }
412 
413  UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
414  if(bundlePoses.size() == bundlePoses_.size()+1)
415  {
416  if(!bundlePoses.rbegin()->second.isNull())
417  {
418  if(sbaOutliers.size())
419  {
420  std::vector<int> newInliers(regInfo.inliersIDs.size());
421  int oi=0;
422  for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
423  {
424  if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
425  {
426  newInliers[oi++] = regInfo.inliersIDs[i];
427  }
428  }
429  newInliers.resize(oi);
430  UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
431  regInfo.inliers = (int)newInliers.size();
432  regInfo.inliersIDs = newInliers;
433  }
434  if(regInfo.inliers < regPipeline_->getMinVisualCorrespondences())
435  {
436  regInfo.rejectedMsg = uFormat("Too low inliers after bundle adjustment: %d<%d", regInfo.inliers, regPipeline_->getMinVisualCorrespondences());
437  transform.setNull();
438  }
439  else
440  {
441  transform = bundlePoses.rbegin()->second;
442  bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
443  }
444  }
445  UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
446  }
447  else
448  {
449  UWARN("Local bundle adjustment failed! transform is not refined.");
450  }
451  }
452  }
453 
454  if(!transform.isNull())
455  {
456  // make it incremental
457  transform = this->getPose().inverse() * transform;
458  }
459  }
460 
461  if(transform.isNull())
462  {
463  if(guessIteration == 1)
464  {
465  UWARN("Trial with no guess still fail.");
466  }
467  if(!regInfo.rejectedMsg.empty())
468  {
469  UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
470  }
471  else
472  {
473  UWARN("Unknown registration error");
474  }
475  }
476  else if(guessIteration == 1)
477  {
478  UWARN("Trial with no guess succeeded!");
479  }
480  }
481 
482  if(!transform.isNull())
483  {
484  output = transform;
485 
486  bool modified = false;
487  Transform newFramePose = this->getPose()*output;
488 
489  // fields to update
490  LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
491  std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
492  std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
493  std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
494 
495  bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
496  (keyFrameThr_ == 0.0f ||
497  visKeyFrameThr_ == 0 ||
498  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
499  regInfo.inliers <= visKeyFrameThr_);
500  bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
501 
502  addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
503  addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
504 
505  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());
506  if(addKeyFrame)
507  {
508  //Visual
509  int added = 0;
510  int removed = 0;
511  UTimer tmpTimer;
512 
513  UDEBUG("Update local map");
514 
515  // update local map
516  UASSERT(mapWords.size() == mapPoints.size());
517  UASSERT(mapPoints.size() == mapDescriptors.size());
518  UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
519 
520  std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
521  if(bundleAdjustment_>0)
522  {
523  bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
524  UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end());
525  bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first));
526  uInsert(bundlePoses_, bundlePoses);
527  UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
528  bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
529  iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
530 
531  // update local map 3D points (if bundle adjustment was done)
532  for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
533  {
534  UASSERT(mapPoints.count(iter->first) == 1);
535  //UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
536  mapPoints.find(iter->first)->second = iter->second;
537  }
538  }
539 
540  // sort by feature response
541  std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
542  UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
543  std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
544  std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
545  UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
546  std::set<int> seenStatusUpdated;
547  Transform invLocalTransform;
548  if(bundleAdjustment_>0)
549  {
550  if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
551  {
552  invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
553  }
554  else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
555  {
556  invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
557  }
558  else
559  {
560  UFATAL("no valid camera model!");
561  }
562  }
563  for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
564  {
565  if(util3d::isFinite(iter->second))
566  {
567  if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
568  {
569  newIds.insert(
570  std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
571  std::make_pair(iter->first,
572  std::make_pair(iter2D->second,
573  std::make_pair(iter->second, iterDesc->second)))));
574  }
575  else if(bundleAdjustment_>0)
576  {
577  if(lastFrame_->getWords().count(iter->first) == 1)
578  {
579  std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
580  if(iterKpts!=mapWords.end())
581  {
582  iterKpts->second.octave = iter2D->second.octave;
583  }
584 
585  UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
586  iterBundlePosesRef->second += 1;
587 
588  //move back point in camera frame (to get depth along z)
589  cv::Point3f pt3d = util3d::transformPoint(iter->second, invLocalTransform);
590  if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
591  {
592  std::map<int, cv::Point3f> framePt;
593  framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
594  bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
595  }
596  else
597  {
598  bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, pt3d.z)));
599  }
600  }
601  }
602  }
603  }
604  UDEBUG("newIds=%d", (int)newIds.size());
605 
606  int lastFrameOldestNewId = lastFrameOldestNewId_;
607  lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
608  for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
609  iter!=newIds.rend();
610  ++iter)
611  {
612  if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
613  {
614  if(bundleAdjustment_>0)
615  {
616  if(lastFrame_->getWords().count(iter->second.first) == 1)
617  {
618  UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
619  iterBundlePosesRef->second += 1;
620 
621  //move back point in camera frame (to get depth along z)
622  cv::Point3f pt3d = util3d::transformPoint(iter->second.second.second.first, invLocalTransform);
623  if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
624  {
625  std::map<int, cv::Point3f> framePt;
626  framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
627  bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
628  }
629  else
630  {
631  bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, pt3d.z)));
632  }
633  }
634  }
635 
636  mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
637  mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
638  mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
639  if(lastFrameOldestNewId_ > iter->second.first)
640  {
641  lastFrameOldestNewId_ = iter->second.first;
642  }
643  ++added;
644  }
645  }
646 
647  // remove words in map if max size is reached
648  if((int)mapPoints.size() > maximumMapSize_)
649  {
650  // remove oldest outliers first
651  std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
652  std::vector<int> ids = regInfo.matchesIDs;
653  if(regInfo.projectedIDs.size())
654  {
655  ids.resize(ids.size() + regInfo.projectedIDs.size());
656  int oi=0;
657  for(unsigned int i=0; i<regInfo.projectedIDs.size(); ++i)
658  {
659  if(regInfo.projectedIDs[i]>=lastFrameOldestNewId)
660  {
661  ids[regInfo.matchesIDs.size()+oi++] = regInfo.projectedIDs[i];
662  }
663  }
664  ids.resize(regInfo.matchesIDs.size()+oi);
665  UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
666  }
667  for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
668  {
669  int id = ids.at(i);
670  if(inliers.find(id) == inliers.end())
671  {
672  std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(id);
673  if(iterRef != bundleWordReferences_.end())
674  {
675  for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
676  {
677  if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
678  {
679  bundlePoseReferences_.at(iterFrame->first) -= 1;
680  }
681  }
682  bundleWordReferences_.erase(iterRef);
683  }
684 
685  mapPoints.erase(id);
686  mapDescriptors.erase(id);
687  mapWords.erase(id);
688  ++removed;
689  }
690  }
691 
692  // remove oldest first
693  std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
694  std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
695  for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
696  iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
697  {
698  if(inliers.find(iter->first) == inliers.end())
699  {
700  std::map<int, std::map<int, cv::Point3f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
701  if(iterRef != bundleWordReferences_.end())
702  {
703  for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
704  {
705  if(bundlePoseReferences_.find(iterFrame->first) != bundlePoseReferences_.end())
706  {
707  bundlePoseReferences_.at(iterFrame->first) -= 1;
708  }
709  }
710  bundleWordReferences_.erase(iterRef);
711  }
712 
713  mapPoints.erase(iter++);
714  mapDescriptors.erase(iterMapDescriptors++);
715  mapWords.erase(iterMapWords++);
716  ++removed;
717  }
718  else
719  {
720  ++iter;
721  ++iterMapDescriptors;
722  ++iterMapWords;
723  }
724  }
725 
726  Link * previousLink = 0;
727  for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
728  {
729  if(iter->second <= 0)
730  {
731  if(previousLink == 0 || bundleLinks_.find(iter->first) != bundleLinks_.end())
732  {
733  if(previousLink)
734  {
735  UASSERT(previousLink->to() == iter->first);
736  *previousLink = previousLink->merge(bundleLinks_.find(iter->first)->second, previousLink->type());
737  }
738  UASSERT(bundlePoses_.erase(iter->first) == 1);
739  bundleLinks_.erase(iter->first);
740  bundleModels_.erase(iter->first);
741  bundlePoseReferences_.erase(iter++);
742  }
743  }
744  else
745  {
746  previousLink=0;
747  if(bundleLinks_.find(iter->first) != bundleLinks_.end())
748  {
749  previousLink = &bundleLinks_.find(iter->first)->second;
750  }
751  ++iter;
752  }
753  }
754  }
755 
756  if(added || removed)
757  {
758  modified = true;
759  }
760  UDEBUG("Update local features map = %fs", tmpTimer.ticks());
761 
762  // Geometric
763  UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
764  UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
765 
766  if(lastFrame_->sensorData().laserScanRaw().size())
767  {
768  pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
769  pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
770 
771  pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
772  int newPoints;
773  if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
774  {
775  frameCloudNormalsIndices = util3d::subtractFiltering(
776  frameCloudNormals,
777  pcl::IndicesPtr(new std::vector<int>),
778  mapCloudNormals,
779  pcl::IndicesPtr(new std::vector<int>),
780  scanSubtractRadius_,
781  scanSubtractAngle_);
782  newPoints = frameCloudNormalsIndices->size();
783  }
784  else
785  {
786  newPoints = mapCloudNormals->size();
787  }
788 
789  if(newPoints)
790  {
791  scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
792 
793  //remove points if too big
794  UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
795  (int)scansBuffer_.size(),
796  int(mapCloudNormals->size()),
797  newPoints,
798  scanMaximumMapSize_);
799 
800  if(scansBuffer_.size() > 1 &&
801  int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
802  {
803  //regenerate the local map
804  mapCloudNormals->clear();
805  std::list<int> toRemove;
806  int i = int(scansBuffer_.size())-1;
807  for(; i>=0; --i)
808  {
809  int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
810  if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
811  i == 0)
812  {
813  *mapCloudNormals += *scansBuffer_[i].first;
814  break;
815  }
816  else
817  {
818  if(scansBuffer_[i].second->size())
819  {
820  pcl::PointCloud<pcl::PointNormal> tmp;
821  pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
822  *mapCloudNormals += tmp;
823  }
824  else
825  {
826  *mapCloudNormals += *scansBuffer_[i].first;
827  }
828  }
829  }
830  // remove old clouds
831  if(i > 0)
832  {
833  std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
834  int oi = 0;
835  for(; i<(int)scansBuffer_.size(); ++i)
836  {
837  UASSERT(oi < (int)scansTmp.size());
838  scansTmp[oi++] = scansBuffer_[i];
839  }
840  scansBuffer_ = scansTmp;
841  }
842  }
843  else
844  {
845  // just append the last cloud
846  if(scansBuffer_.back().second->size())
847  {
848  pcl::PointCloud<pcl::PointNormal> tmp;
849  pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
850  *mapCloudNormals += tmp;
851  }
852  else
853  {
854  *mapCloudNormals += *scansBuffer_.back().first;
855  }
856  }
857  if(mapScan.is2d())
858  {
859  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
860  mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYNormal);
861  }
862  else
863  {
864  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
865  mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZNormal);
866  }
867  modified=true;
868  }
869  }
870  UDEBUG("Update local scan map = %fs", tmpTimer.ticks());
871  }
872 
873  if(modified)
874  {
875  *map_ = tmpMap;
876 
877  if(mapScan.is2d())
878  {
879 
880  map_->sensorData().setLaserScanRaw(
881  LaserScan(
882  mapScan.data(),
883  0,
884  0.0f,
885  mapScan.format(),
886  Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
887  }
888  else
889  {
890  map_->sensorData().setLaserScanRaw(
891  LaserScan(
892  mapScan.data(),
893  0,
894  0.0f,
895  mapScan.format(),
896  newFramePose.translation()));
897  }
898 
899  map_->setWords(mapWords);
900  map_->setWords3(mapPoints);
901  map_->setWordsDescriptors(mapDescriptors);
902  }
903  }
904 
905  if(info)
906  {
907  // use tmpMap instead of map_ to make sure that correspondences with the new frame matches
908  info->localMapSize = (int)tmpMap.getWords3().size();
909  info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
910  if(this->isInfoDataFilled())
911  {
912  info->localMap = uMultimapToMap(tmpMap.getWords3());
913  info->localScanMap = tmpMap.sensorData().laserScanRaw();
914  }
915  }
916  }
917  else
918  {
919  // just generate keypoints for the new signature
920  if(regPipeline_->isImageRequired())
921  {
922  Signature dummy;
923  regPipeline_->computeTransformationMod(
924  *lastFrame_,
925  dummy);
926  }
927 
928  data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());
929 
930  // a very high variance tells that the new pose is not linked with the previous one
931  regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
932 
933  bool frameValid = false;
934  Transform newFramePose = this->getPose(); // initial pose may be not identity...
935  if(regPipeline_->isImageRequired())
936  {
937  if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
938  {
939  frameValid = true;
940  // update local map
941  UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
942  UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
943 
944  std::multimap<int, cv::KeyPoint> words;
945  std::multimap<int, cv::Point3f> transformedPoints;
946  std::multimap<int, int> mapPointWeights;
947  std::multimap<int, cv::Mat> descriptors;
948  UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
949  std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
950  std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
951  for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
952  iter != lastFrame_->getWords3().end();
953  ++iter, ++descIter, ++wordsIter)
954  {
955  if (util3d::isFinite(iter->second))
956  {
957  words.insert(*wordsIter);
958  transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
959  mapPointWeights.insert(std::make_pair(iter->first, 0));
960  descriptors.insert(*descIter);
961  }
962  }
963 
964  if(bundleAdjustment_>0)
965  {
966  Transform invLocalTransform;
967  if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
968  {
969  invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
970  }
971  else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
972  {
973  invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
974  }
975  else
976  {
977  UFATAL("no valid camera model!");
978  }
979 
980  // update bundleWordReferences_: used for bundle adjustment
981  for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
982  {
983  if(words.count(iter->first) == 1)
984  {
985  UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
986  std::map<int, cv::Point3f> framePt;
987 
988  //get depth
989  float d = 0.0f;
990  if(lastFrame_->getWords3().count(iter->first) == 1)
991  {
992  //move back point in camera frame (to get depth along z)
993  cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform);
994  d = pt3d.z;
995  }
996 
997 
998  framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.pt.x, iter->second.pt.y, d)));
999  bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
1000  }
1001  }
1002 
1003  bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
1004 
1005  CameraModel model;
1006  if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
1007  {
1008  model = lastFrame_->sensorData().cameraModels()[0];
1009  }
1010  else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
1011  {
1012  model = lastFrame_->sensorData().stereoCameraModel().left();
1013  // Set Tx for stereo BA
1014  model = CameraModel(model.fx(),
1015  model.fy(),
1016  model.cx(),
1017  model.cy(),
1018  model.localTransform(),
1019  -lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
1020  }
1021  else
1022  {
1023  UFATAL("invalid camera model!");
1024  }
1025  bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
1026  bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
1027  }
1028 
1029  map_->setWords(words);
1030  map_->setWords3(transformedPoints);
1031  map_->setWordsDescriptors(descriptors);
1032  addKeyFrame = true;
1033  }
1034  else
1035  {
1036  UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
1037  }
1038  }
1039  if(regPipeline_->isScanRequired())
1040  {
1041  if (lastFrame_->sensorData().laserScanRaw().size())
1042  {
1043  frameValid = true;
1044  pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
1045  scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
1046  if(lastFrame_->sensorData().laserScanRaw().is2d())
1047  {
1048  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
1049  map_->sensorData().setLaserScanRaw(
1050  LaserScan(
1051  util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
1052  0,
1053  0.0f,
1054  LaserScan::kXYNormal,
1055  Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
1056  }
1057  else
1058  {
1059  Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
1060  map_->sensorData().setLaserScanRaw(
1061  LaserScan(
1062  util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
1063  0,
1064  0.0f,
1065  LaserScan::kXYZNormal,
1066  newFramePose.translation()));
1067  }
1068  addKeyFrame = true;
1069  }
1070  else
1071  {
1072  UWARN("Missing scan to initialize odometry.");
1073  }
1074  }
1075 
1076  if (frameValid)
1077  {
1078  // We initialized the local map
1079  output.setIdentity();
1080  }
1081 
1082  if(info)
1083  {
1084  info->localMapSize = (int)map_->getWords3().size();
1085  info->localScanMapSize = map_->sensorData().laserScanRaw().size();
1086 
1087  if(this->isInfoDataFilled())
1088  {
1089  info->localMap = uMultimapToMap(map_->getWords3());
1090  info->localScanMap = map_->sensorData().laserScanRaw();
1091  }
1092  }
1093  }
1094 
1095  map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat()); // clear sensorData features
1096 
1097  nFeatures = lastFrame_->getWords().size();
1098  if(this->isInfoDataFilled() && info)
1099  {
1100  if(regPipeline_->isImageRequired())
1101  {
1102  info->words = lastFrame_->getWords();
1103  }
1104  }
1105  }
1106 
1107  if(info)
1108  {
1109  info->features = nFeatures;
1110  info->localKeyFrames = (int)bundlePoses_.size();
1111  info->keyFrameAdded = addKeyFrame;
1112  info->localBundleOutliers = totalBundleOutliers;
1113  info->localBundleConstraints = totalBundleWordReferencesUsed;
1114  info->localBundleTime = bundleTime;
1115 
1116  if(this->isInfoDataFilled())
1117  {
1118  info->reg = regInfo;
1119  }
1120  else
1121  {
1122  info->reg = regInfo.copyWithoutData();
1123  }
1124  }
1125 
1126  UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
1127  timer.elapsed(),
1128  output.isNull()?"true":"false",
1129  nFeatures,
1130  regInfo.inliers,
1131  regInfo.matches,
1132  regInfo.covariance.at<double>(0,0),
1133  regInfo.covariance.at<double>(5,5),
1134  regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
1135  regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().size():0);
1136 
1137  return output;
1138 }
1139 
1140 } // namespace rtabmap
rtabmap::Signature::getWords3
const std::multimap< int, cv::Point3f > & getWords3() const
Definition: Signature.h:128
d
d
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
rtabmap::Registration::getMinVisualCorrespondences
int getMinVisualCorrespondences() const
Definition: Registration.cpp:129
rtabmap::util3d::subtractFiltering
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP subtractFiltering(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &substractCloud, float radiusSearch, int minNeighborsInRadius=1)
Definition: util3d_filtering.cpp:860
rtabmap::OdometryF2M::parameters_
ParametersMap parameters_
Definition: OdometryF2M.h:84
rtabmap::Odometry::framesProcessed
unsigned int framesProcessed() const
Definition: Odometry.h:75
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo::localBundleModels
std::map< int, CameraModel > localBundleModels
Definition: OdometryInfo.h:100
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::OdometryF2M::OdometryF2M
OdometryF2M(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryF2M.cpp:59
rtabmap::Signature::getWords
const std::multimap< int, cv::KeyPoint > & getWords() const
Definition: Signature.h:108
rtabmap::Signature::setWords3
void setWords3(const std::multimap< int, cv::Point3f > &words3)
Definition: Signature.h:115
f
f
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:72
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:182
rtabmap::Signature::setWords
void setWords(const std::multimap< int, cv::KeyPoint > &words)
Definition: Signature.cpp:231
util3d_motion_estimation.h
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:188
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:116
rtabmap::OdometryF2M::regPipeline_
Registration * regPipeline_
Definition: OdometryF2M.h:71
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::Transform::translation
Transform translation() const
Definition: Transform.cpp:182
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:229
rtabmap::OdometryF2M::sba_
Optimizer * sba_
Definition: OdometryF2M.h:83
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::OdometryF2M::computeTransform
virtual Transform computeTransform(SensorData &data, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryF2M.cpp:172
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:227
rtabmap::Signature::getWordsDescriptors
const std::multimap< int, cv::Mat > & getWordsDescriptors() const
Definition: Signature.h:111
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:71
UMath.h
Basic mathematics functions.
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:70
rtabmap::SensorData::setId
void setId(int id)
Definition: SensorData.h:153
UConversion.h
Some conversion functions.
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:46
glm::step
GLM_FUNC_DECL genType step(genType const &edge, genType const &x)
UFATAL
#define UFATAL(...)
util3d_registration.h
util3d_correspondences.h
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::isValid
bool isValid() const
Definition: SensorData.h:134
rtabmap::OdometryF2M::lastFrame_
Signature * lastFrame_
Definition: OdometryF2M.h:73
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
rtabmap::OdometryF2M::bundlePoseReferences_
std::map< int, int > bundlePoseReferences_
Definition: OdometryF2M.h:81
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:96
rtabmap::OdometryF2M::scansBuffer_
std::vector< std::pair< pcl::PointCloud< pcl::PointNormal >::Ptr, pcl::IndicesPtr > > scansBuffer_
Definition: OdometryF2M.h:75
rtabmap::RegistrationInfo::projectedIDs
std::vector< int > projectedIDs
Definition: RegistrationInfo.h:73
util3d_filtering.h
uMultimapToMap
std::map< K, V > uMultimapToMap(const std::multimap< K, V > &m)
Definition: UStl.h:496
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:99
rtabmap::OdometryF2M::bundlePoses_
std::map< int, Transform > bundlePoses_
Definition: OdometryF2M.h:78
rtabmap::Registration::parseParameters
virtual void parseParameters(const ParametersMap &parameters)
Definition: Registration.cpp:78
rtabmap::OdometryInfo::localBundlePoses
std::map< int, Transform > localBundlePoses
Definition: OdometryInfo.h:99
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const std::multimap< int, cv::Mat > &descriptors)
Definition: Signature.h:112
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
util3d_transforms.h
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:38
rtabmap::OdometryF2M::map_
Signature * map_
Definition: OdometryF2M.h:72
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, cv::Point3f > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:362
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:115
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true)
Definition: Graph.cpp:825
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
UWARN
#define UWARN(...)
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:89
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::RegistrationInfo::matchesIDs
std::vector< int > matchesIDs
Definition: RegistrationInfo.h:72
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:91
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::OdometryF2M::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryF2M.cpp:156
rtabmap::OdometryF2M::bundleLinks_
std::multimap< int, Link > bundleLinks_
Definition: OdometryF2M.h:79
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:90
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:67
rtabmap::OdometryF2M::bundleModels_
std::map< int, CameraModel > bundleModels_
Definition: OdometryF2M.h:80
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::OdometryF2M::bundleWordReferences_
std::map< int, std::map< int, cv::Point3f > > bundleWordReferences_
Definition: OdometryF2M.h:77
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:828
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:228
rtabmap::RegistrationInfo::copyWithoutData
RegistrationInfo copyWithoutData() const
Definition: RegistrationInfo.h:49
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109
rtabmap::SensorData::setLaserScanRaw
void setLaserScanRaw(const LaserScan &laserScanRaw)
Definition: SensorData.h:166
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32