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

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