Odometry.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/Odometry.h"
29 #include <rtabmap/core/odometry/OdometryF2M.h>
30 #include "rtabmap/core/odometry/OdometryF2F.h"
31 #include "rtabmap/core/odometry/OdometryFovis.h"
32 #include "rtabmap/core/odometry/OdometryViso2.h"
33 #include "rtabmap/core/odometry/OdometryDVO.h"
34 #include "rtabmap/core/odometry/OdometryOkvis.h"
35 #include "rtabmap/core/odometry/OdometryORBSLAM2.h"
36 #include "rtabmap/core/odometry/OdometryLOAM.h"
37 #include "rtabmap/core/odometry/OdometryMSCKF.h"
38 #include "rtabmap/core/odometry/OdometryVINS.h"
39 #include "rtabmap/core/OdometryInfo.h"
40 #include "rtabmap/core/util3d.h"
41 #include "rtabmap/core/util3d_mapping.h"
42 #include "rtabmap/core/util3d_filtering.h"
43 #include "rtabmap/utilite/ULogger.h"
44 #include "rtabmap/utilite/UTimer.h"
45 #include "rtabmap/utilite/UConversion.h"
46 #include "rtabmap/utilite/UProcessInfo.h"
47 #include "rtabmap/core/ParticleFilter.h"
48 #include "rtabmap/core/util2d.h"
49 
50 #include <pcl/pcl_base.h>
51 
52 namespace rtabmap {
53 
54 Odometry * Odometry::create(const ParametersMap & parameters)
55 {
56  int odomTypeInt = Parameters::defaultOdomStrategy();
57  Parameters::parse(parameters, Parameters::kOdomStrategy(), odomTypeInt);
58  Odometry::Type type = (Odometry::Type)odomTypeInt;
59  return create(type, parameters);
60 }
61 
62 Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & parameters)
63 {
64  UDEBUG("type=%d", (int)type);
65  Odometry * odometry = 0;
66  switch(type)
67  {
68  case Odometry::kTypeF2M:
69  odometry = new OdometryF2M(parameters);
70  break;
71  case Odometry::kTypeF2F:
72  odometry = new OdometryF2F(parameters);
73  break;
74  case Odometry::kTypeFovis:
75  odometry = new OdometryFovis(parameters);
76  break;
77  case Odometry::kTypeViso2:
78  odometry = new OdometryViso2(parameters);
79  break;
80  case Odometry::kTypeDVO:
81  odometry = new OdometryDVO(parameters);
82  break;
83  case Odometry::kTypeORBSLAM2:
84  odometry = new OdometryORBSLAM2(parameters);
85  break;
86  case Odometry::kTypeOkvis:
87  odometry = new OdometryOkvis(parameters);
88  break;
89  case Odometry::kTypeLOAM:
90  odometry = new OdometryLOAM(parameters);
91  break;
92  case Odometry::kTypeMSCKF:
93  odometry = new OdometryMSCKF(parameters);
94  break;
95  case Odometry::kTypeVINS:
96  odometry = new OdometryVINS(parameters);
97  break;
98  default:
99  UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
100  odometry = new OdometryF2M(parameters);
101  type = Odometry::kTypeF2M;
102  break;
103  }
104  return odometry;
105 }
106 
107 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
108  _resetCountdown(Parameters::defaultOdomResetCountdown()),
109  _force3DoF(Parameters::defaultRegForce3DoF()),
110  _holonomic(Parameters::defaultOdomHolonomic()),
111  guessFromMotion_(Parameters::defaultOdomGuessMotion()),
112  guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
113  _filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
114  _particleSize(Parameters::defaultOdomParticleSize()),
115  _particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
116  _particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
117  _particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
118  _particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
119  _fillInfoData(Parameters::defaultOdomFillInfoData()),
120  _kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
121  _kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
122  _imageDecimation(Parameters::defaultOdomImageDecimation()),
123  _alignWithGround(Parameters::defaultOdomAlignWithGround()),
124  _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
125  _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
126  _pose(Transform::getIdentity()),
127  _resetCurrentCount(0),
128  previousStamp_(0),
129  distanceTravelled_(0),
130  framesProcessed_(0)
131 {
132  Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
133 
134  Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
135  Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
136  Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
137  Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
138  Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
139  Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
140  Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
141  Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
142  Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
143  Parameters::parse(parameters, Parameters::kOdomParticleNoiseR(), _particleNoiseR);
144  Parameters::parse(parameters, Parameters::kOdomParticleLambdaR(), _particleLambdaR);
145  UASSERT(_particleNoiseT>0);
146  UASSERT(_particleLambdaT>0);
147  UASSERT(_particleNoiseR>0);
148  UASSERT(_particleLambdaR>0);
149  Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
150  Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
151  Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
152  Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
153  Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
154  Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
155 
156  if(_imageDecimation == 0)
157  {
158  _imageDecimation = 1;
159  }
160 
161  if(_filteringStrategy == 2)
162  {
163  // Initialize the Particle filters
164  particleFilters_.resize(6);
165  for(unsigned int i = 0; i<particleFilters_.size(); ++i)
166  {
167  if(i<3)
168  {
169  particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
170  }
171  else
172  {
173  particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
174  }
175  }
176  }
177  else if(_filteringStrategy == 1)
178  {
179  initKalmanFilter();
180  }
181 }
182 
183 Odometry::~Odometry()
184 {
185  for(unsigned int i=0; i<particleFilters_.size(); ++i)
186  {
187  delete particleFilters_[i];
188  }
189 }
190 
191 void Odometry::reset(const Transform & initialPose)
192 {
193  UASSERT(!initialPose.isNull());
194  previousVelocities_.clear();
195  velocityGuess_.setNull();
196  previousGroundTruthPose_.setNull();
197  _resetCurrentCount = 0;
198  previousStamp_ = 0;
199  distanceTravelled_ = 0;
200  framesProcessed_ = 0;
201  imuLastTransform_.setNull();
202  imus_.clear();
203  if(_force3DoF || particleFilters_.size())
204  {
205  float x,y,z, roll,pitch,yaw;
206  initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
207 
208  if(_force3DoF)
209  {
210  if(z != 0.0f || roll != 0.0f || pitch != 0.0f)
211  {
212  UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
213  }
214  z = 0;
215  roll = 0;
216  pitch = 0;
217  Transform pose(x, y, z, roll, pitch, yaw);
218  _pose = pose;
219  }
220  else
221  {
222  _pose = initialPose;
223  }
224 
225  if(particleFilters_.size())
226  {
227  UASSERT(particleFilters_.size() == 6);
228  particleFilters_[0]->init(x);
229  particleFilters_[1]->init(y);
230  particleFilters_[2]->init(z);
231  particleFilters_[3]->init(roll);
232  particleFilters_[4]->init(pitch);
233  particleFilters_[5]->init(yaw);
234  }
235 
236  if(_filteringStrategy == 1)
237  {
238  initKalmanFilter(initialPose);
239  }
240  }
241  else
242  {
243  _pose = initialPose;
244  }
245 }
246 
247 const Transform & Odometry::previousVelocityTransform() const
248 {
249  return getVelocityGuess();
250 }
251 
252 Transform getMeanVelocity(const std::list<std::pair<std::vector<float>, double> > & transforms)
253 {
254  if(transforms.size())
255  {
256  float tvx=0.0f,tvy=0.0f,tvz=0.0f, tvroll=0.0f,tvpitch=0.0f,tvyaw=0.0f;
257  for(std::list<std::pair<std::vector<float>, double> >::const_iterator iter=transforms.begin(); iter!=transforms.end(); ++iter)
258  {
259  UASSERT(iter->first.size() == 6);
260  tvx+=iter->first[0];
261  tvy+=iter->first[1];
262  tvz+=iter->first[2];
263  tvroll+=iter->first[3];
264  tvpitch+=iter->first[4];
265  tvyaw+=iter->first[5];
266  }
267  tvx/=float(transforms.size());
268  tvy/=float(transforms.size());
269  tvz/=float(transforms.size());
270  tvroll/=float(transforms.size());
271  tvpitch/=float(transforms.size());
272  tvyaw/=float(transforms.size());
273  return Transform(tvx, tvy, tvz, tvroll, tvpitch, tvyaw);
274  }
275  return Transform();
276 }
277 
278 Transform Odometry::process(SensorData & data, OdometryInfo * info)
279 {
280  return process(data, Transform(), info);
281 }
282 
283 Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
284 {
285  UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
286 
287  if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
288  {
289  if(data.stereoCameraModel().isValidForRectification())
290  {
291  if(!stereoModel_.isRectificationMapInitialized() ||
292  stereoModel_.left().imageSize() != data.stereoCameraModel().left().imageSize())
293  {
294  stereoModel_ = data.stereoCameraModel();
295  stereoModel_.initRectificationMap();
296  if(stereoModel_.isRectificationMapInitialized())
297  {
298  UWARN("%s parameter is set to false but the selected odometry approach cannot "
299  "process raw images. We will rectify them for convenience.",
300  Parameters::kRtabmapImagesAlreadyRectified().c_str());
301  }
302  else
303  {
304  UERROR("Odometry approach chosen cannot process raw images (not rectified images) and we cannot rectify them. "
305  "Make sure images are rectified, and set %s parameter back to true.",
306  Parameters::kRtabmapImagesAlreadyRectified().c_str());
307  }
308  }
309  if(stereoModel_.isRectificationMapInitialized())
310  {
311  data.setStereoImage(
312  stereoModel_.left().rectifyImage(data.imageRaw()),
313  stereoModel_.right().rectifyImage(data.rightRaw()),
314  stereoModel_,
315  false);
316  }
317  }
318  else
319  {
320  UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
321  "are rectified, and set %s parameter back to true.",
322  Parameters::kRtabmapImagesAlreadyRectified().c_str());
323  }
324  }
325 
326  // Ground alignment
327  if(_pose.isIdentity() && _alignWithGround)
328  {
329  if(data.depthOrRightRaw().empty())
330  {
331  UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
332  }
333  else
334  {
335  UTimer alignTimer;
336  pcl::IndicesPtr indices(new std::vector<int>);
337  pcl::IndicesPtr ground, obstacles;
338  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
339  bool success = false;
340  if(indices->size())
341  {
342  cloud = util3d::voxelize(cloud, indices, 0.01);
343  util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
344  if(ground->size())
345  {
346  pcl::ModelCoefficients coefficients;
347  util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
348  if(coefficients.values.at(3) >= 0)
349  {
350  UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
351  coefficients.values.at(0),
352  coefficients.values.at(1),
353  coefficients.values.at(2),
354  coefficients.values.at(3),
355  alignTimer.ticks());
356  }
357  else
358  {
359  UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
360  coefficients.values.at(0),
361  coefficients.values.at(1),
362  coefficients.values.at(2),
363  coefficients.values.at(3),
364  alignTimer.ticks());
365  }
366  Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
367  Eigen::Vector3f z(0,0,1);
368  //get rotation from z to n;
369  Eigen::Matrix3f R;
370  R = Eigen::Quaternionf().setFromTwoVectors(n,z);
371  Transform rotation(
372  R(0,0), R(0,1), R(0,2), 0,
373  R(1,0), R(1,1), R(1,2), 0,
374  R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
375  this->reset(rotation);
376  success = true;
377  }
378  }
379  if(!success)
380  {
381  UERROR("Odometry failed to detect the ground. You have this "
382  "error because parameter \"%s\" is true. "
383  "Make sure the camera is seeing the ground (e.g., tilt ~30 "
384  "degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
385  }
386  }
387  }
388 
389  // cache imu data
390  if(!data.imu().empty())
391  {
392  if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
393  {
394  Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
395  // orientation includes roll and pitch but not yaw in local transform
396  imus_.insert(std::make_pair(data.stamp(), Transform(0,0,data.imu().localTransform().theta()) * orientation*data.imu().localTransform().inverse()));
397  if(imus_.size() > 1000)
398  {
399  imus_.erase(imus_.begin());
400  }
401  }
402  }
403 
404  // KITTI datasets start with stamp=0
405  double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
406  Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
407  if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
408  {
409  if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
410  {
411  UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
412  }
413  else if(_filteringStrategy==1)
414  {
415  UERROR("Kalman filtering is enabled but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, velocityGuess_.prettyPrint().c_str());
416  }
417  dt=0;
418  previousVelocities_.clear();
419  velocityGuess_.setNull();
420  }
421  if(!velocityGuess_.isNull())
422  {
423  if(guessFromMotion_)
424  {
425  if(_filteringStrategy == 1)
426  {
427  // use Kalman predict transform
428  float vx,vy,vz, vroll,vpitch,vyaw;
429  predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
430  guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
431  }
432  else
433  {
434  float vx,vy,vz, vroll,vpitch,vyaw;
435  velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
436  guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
437  }
438  }
439  else if(_filteringStrategy == 1)
440  {
441  predictKalmanFilter(dt);
442  }
443  }
444 
445  Transform imuCurrentTransform;
446  if(!guessIn.isNull())
447  {
448  guess = guessIn;
449  }
450  else if(!data.imu().empty() && !imus_.empty())
451  {
452  // replace orientation guess with IMU (if available)
453  imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
454  if(!imuCurrentTransform.isNull() && !imuLastTransform_.isNull())
455  {
456  Transform orientation = imuLastTransform_.inverse() * imuCurrentTransform;
457  guess = Transform(
458  orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
459  orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
460  orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
461  }
462  }
463 
464  UTimer time;
465  Transform t;
466  if(_imageDecimation > 1 && !data.imageRaw().empty())
467  {
468  // Decimation of images with calibrations
469  SensorData decimatedData = data;
470  cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), _imageDecimation);
471  cv::Mat depthRight = util2d::decimate(decimatedData.depthOrRightRaw(), _imageDecimation);
472  std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
473  for(unsigned int i=0; i<cameraModels.size(); ++i)
474  {
475  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
476  }
477  if(!cameraModels.empty())
478  {
479  decimatedData.setRGBDImage(rgbLeft, depthRight, cameraModels);
480  }
481  else
482  {
483  StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
484  if(stereoModel.isValidForProjection())
485  {
486  stereoModel.scale(1.0/double(_imageDecimation));
487  }
488  decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel);
489  }
490 
491 
492  // compute transform
493  t = this->computeTransform(decimatedData, guess, info);
494 
495  // transform back the keypoints in the original image
496  std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
497  double log2value = log(double(_imageDecimation))/log(2.0);
498  for(unsigned int i=0; i<kpts.size(); ++i)
499  {
500  kpts[i].pt.x *= _imageDecimation;
501  kpts[i].pt.y *= _imageDecimation;
502  kpts[i].size *= _imageDecimation;
503  kpts[i].octave += log2value;
504  }
505  data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
506 
507  if(info)
508  {
509  UASSERT(info->newCorners.size() == info->refCorners.size());
510  for(unsigned int i=0; i<info->newCorners.size(); ++i)
511  {
512  info->refCorners[i].x *= _imageDecimation;
513  info->refCorners[i].y *= _imageDecimation;
514  info->newCorners[i].x *= _imageDecimation;
515  info->newCorners[i].y *= _imageDecimation;
516  }
517  for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
518  {
519  iter->second.pt.x *= _imageDecimation;
520  iter->second.pt.y *= _imageDecimation;
521  iter->second.size *= _imageDecimation;
522  iter->second.octave += log2value;
523  }
524  }
525  }
526  else
527  {
528  t = this->computeTransform(data, guess, info);
529  }
530 
531  if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
532  {
533  return Transform(); // Return null on IMU-only updates
534  }
535 
536  if(info)
537  {
538  info->timeEstimation = time.ticks();
539  info->lost = t.isNull();
540  info->stamp = data.stamp();
541  info->interval = dt;
542  info->transform = t;
543  if(_publishRAMUsage)
544  {
545  info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
546  }
547 
548  if(!data.groundTruth().isNull())
549  {
550  if(!previousGroundTruthPose_.isNull())
551  {
552  info->transformGroundTruth = previousGroundTruthPose_.inverse() * data.groundTruth();
553  }
554  previousGroundTruthPose_ = data.groundTruth();
555  }
556  }
557 
558  if(!t.isNull())
559  {
560  _resetCurrentCount = _resetCountdown;
561 
562  float vx,vy,vz, vroll,vpitch,vyaw;
563  t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
564 
565  // transform to velocity
566  if(dt)
567  {
568  vx /= dt;
569  vy /= dt;
570  vz /= dt;
571  vroll /= dt;
572  vpitch /= dt;
573  vyaw /= dt;
574  }
575 
576  if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
577  {
578  if(_filteringStrategy == 1)
579  {
580  if(velocityGuess_.isNull())
581  {
582  // reset Kalman
583  if(dt)
584  {
585  initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
586  }
587  else
588  {
589  initKalmanFilter(t);
590  }
591  }
592  else
593  {
594  // Kalman filtering
595  updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
596  }
597  }
598  else
599  {
600  if(particleFilters_.size())
601  {
602  // Particle filtering
603  UASSERT(particleFilters_.size()==6);
604  if(velocityGuess_.isNull())
605  {
606  particleFilters_[0]->init(vx);
607  particleFilters_[1]->init(vy);
608  particleFilters_[2]->init(vz);
609  particleFilters_[3]->init(vroll);
610  particleFilters_[4]->init(vpitch);
611  particleFilters_[5]->init(vyaw);
612  }
613  else
614  {
615  vx = particleFilters_[0]->filter(vx);
616  vy = particleFilters_[1]->filter(vy);
617  vyaw = particleFilters_[5]->filter(vyaw);
618 
619  if(!_holonomic)
620  {
621  // arc trajectory around ICR
622  float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
623  if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
624  {
625  vy = tmpY;
626  }
627  else
628  {
629  vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
630  }
631  }
632 
633  if(!_force3DoF)
634  {
635  vz = particleFilters_[2]->filter(vz);
636  vroll = particleFilters_[3]->filter(vroll);
637  vpitch = particleFilters_[4]->filter(vpitch);
638  }
639  }
640 
641  if(info)
642  {
643  info->timeParticleFiltering = time.ticks();
644  }
645  }
646  else if(!_holonomic)
647  {
648  // arc trajectory around ICR
649  vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
650  }
651 
652  if(_force3DoF)
653  {
654  vz = 0.0f;
655  vroll = 0.0f;
656  vpitch = 0.0f;
657  }
658  }
659 
660  if(dt)
661  {
662  t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
663  }
664  else
665  {
666  t = Transform(vx, vy, vz, vroll, vpitch, vyaw);
667  }
668 
669  if(info)
670  {
671  info->transformFiltered = t;
672  }
673  }
674 
675  if(data.stamp() == 0 && framesProcessed_ != 0)
676  {
677  UWARN("Null stamp detected");
678  }
679 
680  previousStamp_ = data.stamp();
681 
682  if(dt)
683  {
684  if(dt >= (guessSmoothingDelay_/2.0) || particleFilters_.size() || _filteringStrategy==1)
685  {
686  velocityGuess_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
687  previousVelocities_.clear();
688  }
689  else
690  {
691  // smooth velocity estimation over the past X seconds
692  std::vector<float> v(6);
693  v[0] = vx;
694  v[1] = vy;
695  v[2] = vz;
696  v[3] = vroll;
697  v[4] = vpitch;
698  v[5] = vyaw;
699  previousVelocities_.push_back(std::make_pair(v, data.stamp()));
700  while(previousVelocities_.size() > 1 && previousVelocities_.front().second < previousVelocities_.back().second-guessSmoothingDelay_)
701  {
702  previousVelocities_.pop_front();
703  }
704  velocityGuess_ = getMeanVelocity(previousVelocities_);
705  }
706  }
707  else
708  {
709  previousVelocities_.clear();
710  velocityGuess_.setNull();
711  }
712 
713  if(info)
714  {
715  distanceTravelled_ += t.getNorm();
716  info->distanceTravelled = distanceTravelled_;
717  info->guessVelocity = velocityGuess_;
718  }
719  ++framesProcessed_;
720 
721  imuLastTransform_ = imuCurrentTransform;
722 
723  return _pose *= t; // update
724  }
725  else if(_resetCurrentCount > 0)
726  {
727  UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
728 
729  --_resetCurrentCount;
730  if(_resetCurrentCount == 0)
731  {
732  UWARN("Odometry automatically reset to latest pose!");
733  this->reset(_pose);
734  }
735  }
736 
737  previousVelocities_.clear();
738  velocityGuess_.setNull();
739  previousStamp_ = 0;
740 
741  return Transform();
742 }
743 
744 void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
745 {
746  UDEBUG("");
747  // See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
748  // See Kalman filter pose/orientation estimation theory: http://campar.in.tum.de/Chair/KalmanFilter
749 
750  // initialize the Kalman filter
751  int nStates = 18; // the number of states (x,y,z,x',y',z',x'',y'',z'',roll,pitch,yaw,roll',pitch',yaw',roll'',pitch'',yaw'')
752  int nMeasurements = 6; // the number of measured states (x',y',z',roll',pitch',yaw')
753  if(_force3DoF)
754  {
755  nStates = 9; // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
756  nMeasurements = 3; // the number of measured states (x',y',yaw')
757  }
758  int nInputs = 0; // the number of action control
759 
760  /* From viso2, measurement covariance
761  * static const boost::array<double, 36> STANDARD_POSE_COVARIANCE =
762  { { 0.1, 0, 0, 0, 0, 0,
763  0, 0.1, 0, 0, 0, 0,
764  0, 0, 0.1, 0, 0, 0,
765  0, 0, 0, 0.17, 0, 0,
766  0, 0, 0, 0, 0.17, 0,
767  0, 0, 0, 0, 0, 0.17 } };
768  static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
769  { { 0.05, 0, 0, 0, 0, 0,
770  0, 0.05, 0, 0, 0, 0,
771  0, 0, 0.05, 0, 0, 0,
772  0, 0, 0, 0.09, 0, 0,
773  0, 0, 0, 0, 0.09, 0,
774  0, 0, 0, 0, 0, 0.09 } };
775  */
776 
777 
778  kalmanFilter_.init(nStates, nMeasurements, nInputs); // init Kalman Filter
779  cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise)); // set process noise
780  cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise)); // set measurement noise
781  cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1)); // error covariance
782 
783  float x,y,z,roll,pitch,yaw;
784  initialPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
785 
786  if(_force3DoF)
787  {
788  /* MEASUREMENT MODEL (velocity) */
789  // [0 0 1 0 0 0 0 0 0]
790  // [0 0 0 1 0 0 0 0 0]
791  // [0 0 0 0 0 0 0 1 0]
792  kalmanFilter_.measurementMatrix.at<float>(0,2) = 1; // x'
793  kalmanFilter_.measurementMatrix.at<float>(1,3) = 1; // y'
794  kalmanFilter_.measurementMatrix.at<float>(2,7) = 1; // yaw'
795 
796  kalmanFilter_.statePost.at<float>(0) = x;
797  kalmanFilter_.statePost.at<float>(1) = y;
798  kalmanFilter_.statePost.at<float>(6) = yaw;
799 
800  kalmanFilter_.statePost.at<float>(2) = vx;
801  kalmanFilter_.statePost.at<float>(3) = vy;
802  kalmanFilter_.statePost.at<float>(7) = vyaw;
803  }
804  else
805  {
806  /* MEASUREMENT MODEL (velocity) */
807  // [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
808  // [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0]
809  // [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]
810  // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0]
811  // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0]
812  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0]
813  kalmanFilter_.measurementMatrix.at<float>(0,3) = 1; // x'
814  kalmanFilter_.measurementMatrix.at<float>(1,4) = 1; // y'
815  kalmanFilter_.measurementMatrix.at<float>(2,5) = 1; // z'
816  kalmanFilter_.measurementMatrix.at<float>(3,12) = 1; // roll'
817  kalmanFilter_.measurementMatrix.at<float>(4,13) = 1; // pitch'
818  kalmanFilter_.measurementMatrix.at<float>(5,14) = 1; // yaw'
819 
820  kalmanFilter_.statePost.at<float>(0) = x;
821  kalmanFilter_.statePost.at<float>(1) = y;
822  kalmanFilter_.statePost.at<float>(2) = z;
823  kalmanFilter_.statePost.at<float>(9) = roll;
824  kalmanFilter_.statePost.at<float>(10) = pitch;
825  kalmanFilter_.statePost.at<float>(11) = yaw;
826 
827  kalmanFilter_.statePost.at<float>(3) = vx;
828  kalmanFilter_.statePost.at<float>(4) = vy;
829  kalmanFilter_.statePost.at<float>(5) = vz;
830  kalmanFilter_.statePost.at<float>(12) = vroll;
831  kalmanFilter_.statePost.at<float>(13) = vpitch;
832  kalmanFilter_.statePost.at<float>(14) = vyaw;
833  }
834 }
835 
836 void Odometry::predictKalmanFilter(float dt, float * vx, float * vy, float * vz, float * vroll, float * vpitch, float * vyaw)
837 {
838  // Set transition matrix with current dt
839  if(_force3DoF)
840  {
841  // 2D:
842  // [1 0 dt 0 dt2 0 0 0 0] x
843  // [0 1 0 dt 0 dt2 0 0 0] y
844  // [0 0 1 0 dt 0 0 0 0] x'
845  // [0 0 0 1 0 dt 0 0 0] y'
846  // [0 0 0 0 1 0 0 0 0] x''
847  // [0 0 0 0 0 0 0 0 0] y''
848  // [0 0 0 0 0 0 1 dt dt2] yaw
849  // [0 0 0 0 0 0 0 1 dt] yaw'
850  // [0 0 0 0 0 0 0 0 1] yaw''
851  kalmanFilter_.transitionMatrix.at<float>(0,2) = dt;
852  kalmanFilter_.transitionMatrix.at<float>(1,3) = dt;
853  kalmanFilter_.transitionMatrix.at<float>(2,4) = dt;
854  kalmanFilter_.transitionMatrix.at<float>(3,5) = dt;
855  kalmanFilter_.transitionMatrix.at<float>(0,4) = 0.5*pow(dt,2);
856  kalmanFilter_.transitionMatrix.at<float>(1,5) = 0.5*pow(dt,2);
857  // orientation
858  kalmanFilter_.transitionMatrix.at<float>(6,7) = dt;
859  kalmanFilter_.transitionMatrix.at<float>(7,8) = dt;
860  kalmanFilter_.transitionMatrix.at<float>(6,8) = 0.5*pow(dt,2);
861  }
862  else
863  {
864  // [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0] x
865  // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] y
866  // [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0] z
867  // [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] x'
868  // [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0] y'
869  // [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] z'
870  // [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0] x''
871  // [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0] y''
872  // [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0] z''
873  // [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
874  // [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
875  // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
876  // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
877  // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
878  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
879  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
880  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
881  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
882  // position
883  kalmanFilter_.transitionMatrix.at<float>(0,3) = dt;
884  kalmanFilter_.transitionMatrix.at<float>(1,4) = dt;
885  kalmanFilter_.transitionMatrix.at<float>(2,5) = dt;
886  kalmanFilter_.transitionMatrix.at<float>(3,6) = dt;
887  kalmanFilter_.transitionMatrix.at<float>(4,7) = dt;
888  kalmanFilter_.transitionMatrix.at<float>(5,8) = dt;
889  kalmanFilter_.transitionMatrix.at<float>(0,6) = 0.5*pow(dt,2);
890  kalmanFilter_.transitionMatrix.at<float>(1,7) = 0.5*pow(dt,2);
891  kalmanFilter_.transitionMatrix.at<float>(2,8) = 0.5*pow(dt,2);
892  // orientation
893  kalmanFilter_.transitionMatrix.at<float>(9,12) = dt;
894  kalmanFilter_.transitionMatrix.at<float>(10,13) = dt;
895  kalmanFilter_.transitionMatrix.at<float>(11,14) = dt;
896  kalmanFilter_.transitionMatrix.at<float>(12,15) = dt;
897  kalmanFilter_.transitionMatrix.at<float>(13,16) = dt;
898  kalmanFilter_.transitionMatrix.at<float>(14,17) = dt;
899  kalmanFilter_.transitionMatrix.at<float>(9,15) = 0.5*pow(dt,2);
900  kalmanFilter_.transitionMatrix.at<float>(10,16) = 0.5*pow(dt,2);
901  kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
902  }
903 
904  // First predict, to update the internal statePre variable
905  UDEBUG("Predict");
906  const cv::Mat & prediction = kalmanFilter_.predict();
907 
908  if(vx)
909  *vx = prediction.at<float>(3); // x'
910  if(vy)
911  *vy = prediction.at<float>(4); // y'
912  if(vz)
913  *vz = _force3DoF?0.0f:prediction.at<float>(5); // z'
914  if(vroll)
915  *vroll = _force3DoF?0.0f:prediction.at<float>(12); // roll'
916  if(vpitch)
917  *vpitch = _force3DoF?0.0f:prediction.at<float>(13); // pitch'
918  if(vyaw)
919  *vyaw = prediction.at<float>(_force3DoF?7:14); // yaw'
920 }
921 
922 void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
923 {
924  // Set measurement to predict
925  cv::Mat measurements;
926  if(!_force3DoF)
927  {
928  measurements = cv::Mat(6,1,CV_32FC1);
929  measurements.at<float>(0) = vx; // x'
930  measurements.at<float>(1) = vy; // y'
931  measurements.at<float>(2) = vz; // z'
932  measurements.at<float>(3) = vroll; // roll'
933  measurements.at<float>(4) = vpitch; // pitch'
934  measurements.at<float>(5) = vyaw; // yaw'
935  }
936  else
937  {
938  measurements = cv::Mat(3,1,CV_32FC1);
939  measurements.at<float>(0) = vx; // x'
940  measurements.at<float>(1) = vy; // y'
941  measurements.at<float>(2) = vyaw; // yaw',
942  }
943 
944  // The "correct" phase that is going to use the predicted value and our measurement
945  UDEBUG("Correct");
946  const cv::Mat & estimated = kalmanFilter_.correct(measurements);
947 
948 
949  vx = estimated.at<float>(3); // x'
950  vy = estimated.at<float>(4); // y'
951  vz = _force3DoF?0.0f:estimated.at<float>(5); // z'
952  vroll = _force3DoF?0.0f:estimated.at<float>(12); // roll'
953  vpitch = _force3DoF?0.0f:estimated.at<float>(13); // pitch'
954  vyaw = estimated.at<float>(_force3DoF?7:14); // yaw'
955 }
956 
957 } /* namespace rtabmap */
glm::log
GLM_FUNC_DECL genType log(genType const &x)
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
rtabmap::Odometry::_imagesAlreadyRectified
bool _imagesAlreadyRectified
Definition: Odometry.h:109
rtabmap::OdometryInfo::refCorners
std::vector< cv::Point2f > refCorners
Definition: OdometryInfo.h:128
rtabmap::Odometry::framesProcessed
unsigned int framesProcessed() const
Definition: Odometry.h:78
rtabmap::Transform::r13
float r13() const
Definition: Transform.h:64
UTimer
Definition: UTimer.h:46
rtabmap::Odometry::_kalmanMeasurementNoise
float _kalmanMeasurementNoise
Definition: Odometry.h:105
rtabmap::Transform::r23
float r23() const
Definition: Transform.h:67
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:295
rtabmap::OdometryInfo::transformGroundTruth
Transform transformGroundTruth
Definition: OdometryInfo.h:113
rtabmap::IMU::orientation
const cv::Vec4d & orientation() const
Definition: IMU.h:52
rtabmap::CameraModel::imageSize
const cv::Size & imageSize() const
Definition: CameraModel.h:119
rtabmap::Odometry::velocityGuess_
Transform velocityGuess_
Definition: Odometry.h:114
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:63
rtabmap::Odometry::kalmanFilter_
cv::KalmanFilter kalmanFilter_
Definition: Odometry.h:121
f
f
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:191
rtabmap::OdometryInfo::transformFiltered
Transform transformFiltered
Definition: OdometryInfo.h:112
rtabmap::StereoCameraModel::isRectificationMapInitialized
bool isRectificationMapInitialized() const
Definition: StereoCameraModel.h:89
rtabmap::Odometry::_particleLambdaR
float _particleLambdaR
Definition: Odometry.h:102
rtabmap::Odometry::framesProcessed_
unsigned int framesProcessed_
Definition: Odometry.h:118
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:380
rtabmap::Odometry::previousStamp_
double previousStamp_
Definition: Odometry.h:112
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:101
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:251
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:252
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::SensorData::setRGBDImage
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:193
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Odometry::_kalmanProcessNoise
float _kalmanProcessNoise
Definition: Odometry.h:104
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:249
UConversion.h
Some conversion functions.
rtabmap::Odometry::canProcessRawImages
virtual bool canProcessRawImages() const
Definition: Odometry.h:69
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::Odometry::guessSmoothingDelay_
float guessSmoothingDelay_
Definition: Odometry.h:96
rtabmap::SensorData::groundTruth
const Transform & groundTruth() const
Definition: SensorData.h:259
rtabmap::Odometry::_filteringStrategy
int _filteringStrategy
Definition: Odometry.h:97
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
rtabmap::Odometry::_resetCurrentCount
int _resetCurrentCount
Definition: Odometry.h:111
rtabmap::Odometry::predictKalmanFilter
void predictKalmanFilter(float dt, float *vx=0, float *vy=0, float *vz=0, float *vroll=0, float *vpitch=0, float *vyaw=0)
Definition: Odometry.cpp:836
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:278
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:190
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Odometry::distanceTravelled_
float distanceTravelled_
Definition: Odometry.h:117
rtabmap::Odometry::stereoModel_
StereoCameraModel stereoModel_
Definition: Odometry.h:122
util3d_mapping.h
rtabmap::Odometry::~Odometry
virtual ~Odometry()
Definition: Odometry.cpp:183
UASSERT
#define UASSERT(condition)
rtabmap::Transform::theta
float theta() const
Definition: Transform.cpp:162
rtabmap::Odometry::guessFromMotion_
bool guessFromMotion_
Definition: Odometry.h:95
rtabmap::Odometry::updateKalmanFilter
void updateKalmanFilter(float &vx, float &vy, float &vz, float &vroll, float &vpitch, float &vyaw)
Definition: Odometry.cpp:922
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:54
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::util2d::decimate
cv::Mat RTABMAP_EXP decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
util3d_filtering.h
rtabmap::Transform::r12
float r12() const
Definition: Transform.h:63
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
rtabmap::Transform::getTransform
static Transform getTransform(const std::map< double, Transform > &tfBuffer, const double &stamp)
Definition: Transform.cpp:486
rtabmap::Transform::r31
float r31() const
Definition: Transform.h:68
rtabmap::Odometry::previousGroundTruthPose_
Transform previousGroundTruthPose_
Definition: Odometry.h:116
rtabmap::Odometry::computeTransform
virtual Transform computeTransform(SensorData &data, const Transform &guess=Transform(), OdometryInfo *info=0)=0
rtabmap::SensorData::setStereoImage
void setStereoImage(const cv::Mat &left, const cv::Mat &right, const StereoCameraModel &stereoCameraModel, bool clearPreviousData=true)
Definition: SensorData.cpp:270
rtabmap::Odometry::_publishRAMUsage
bool _publishRAMUsage
Definition: Odometry.h:108
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:165
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:155
rtabmap::Transform::r21
float r21() const
Definition: Transform.h:65
rtabmap::Odometry::particleFilters_
std::vector< ParticleFilter * > particleFilters_
Definition: Odometry.h:120
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:157
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::CameraModel::rectifyImage
cv::Mat rectifyImage(const cv::Mat &raw, int interpolation=cv::INTER_LINEAR) const
Definition: CameraModel.cpp:662
rtabmap::getMeanVelocity
Transform getMeanVelocity(const std::list< std::pair< std::vector< float >, double > > &transforms)
Definition: Odometry.cpp:252
rtabmap::Odometry::getVelocityGuess
const Transform & getVelocityGuess() const
Definition: Odometry.h:76
rtabmap::SensorData::imu
const IMU & imu() const
Definition: SensorData.h:269
glm::tan
GLM_FUNC_DECL genType tan(genType const &angle)
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:852
rtabmap::IMU::localTransform
const Transform & localTransform() const
Definition: IMU.h:61
UDEBUG
#define UDEBUG(...)
rtabmap::OdometryInfo::words
std::multimap< int, cv::KeyPoint > words
Definition: OdometryInfo.h:123
glm::pow
GLM_FUNC_DECL genType pow(genType const &base, genType const &exponent)
rtabmap::StereoCameraModel::isValidForRectification
bool isValidForRectification() const
Definition: StereoCameraModel.h:86
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:123
rtabmap::util3d::extractPlane
pcl::IndicesPtr extractPlane(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float distanceThreshold, int maxIterations=100, pcl::ModelCoefficients *coefficientsOut=0)
Definition: util3d_filtering.cpp:1864
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Odometry::_pose
Transform _pose
Definition: Odometry.h:110
rtabmap::Odometry::_particleNoiseR
float _particleNoiseR
Definition: Odometry.h:101
UProcessInfo::getMemoryUsage
static long int getMemoryUsage()
Definition: UProcessInfo.cpp:38
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
UWARN
#define UWARN(...)
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:129
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::Odometry::Odometry
Odometry(const rtabmap::ParametersMap &parameters)
Definition: Odometry.cpp:107
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:519
rtabmap::Odometry::imuLastTransform_
Transform imuLastTransform_
Definition: Odometry.h:115
rtabmap::Odometry::imus_
std::map< double, Transform > imus_
Definition: Odometry.h:123
rtabmap::Odometry::_particleNoiseT
float _particleNoiseT
Definition: Odometry.h:99
rtabmap::Odometry::previousVelocities_
std::list< std::pair< std::vector< float >, double > > previousVelocities_
Definition: Odometry.h:113
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:227
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::Odometry::_alignWithGround
bool _alignWithGround
Definition: Odometry.h:107
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:757
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:250
rtabmap::Odometry::_particleLambdaT
float _particleLambdaT
Definition: Odometry.h:100
rtabmap::Transform::r32
float r32() const
Definition: Transform.h:69
glm::orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
rtabmap::Odometry::initKalmanFilter
void initKalmanFilter(const Transform &initialPose=Transform::getIdentity(), float vx=0.0f, float vy=0.0f, float vz=0.0f, float vroll=0.0f, float vpitch=0.0f, float vyaw=0.0f)
Definition: Odometry.cpp:744

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