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/OdometryORBSLAM3.h"
36 #include "rtabmap/core/odometry/OdometryLOAM.h"
37 #include "rtabmap/core/odometry/OdometryFLOAM.h"
38 #include "rtabmap/core/odometry/OdometryMSCKF.h"
39 #include "rtabmap/core/odometry/OdometryVINS.h"
40 #include "rtabmap/core/odometry/OdometryOpenVINS.h"
41 #include "rtabmap/core/odometry/OdometryOpen3D.h"
42 #include "rtabmap/core/OdometryInfo.h"
43 #include "rtabmap/core/util3d.h"
44 #include "rtabmap/core/util3d_mapping.h"
45 #include "rtabmap/core/util3d_filtering.h"
46 #include "rtabmap/utilite/ULogger.h"
47 #include "rtabmap/utilite/UTimer.h"
48 #include "rtabmap/utilite/UConversion.h"
49 #include "rtabmap/utilite/UProcessInfo.h"
50 #include "rtabmap/core/ParticleFilter.h"
51 #include "rtabmap/core/util2d.h"
52 
53 #include <pcl/pcl_base.h>
54 #include <rtabmap/core/odometry/OdometryORBSLAM2.h>
55 
56 namespace rtabmap {
57 
58 Odometry * Odometry::create(const ParametersMap & parameters)
59 {
60  int odomTypeInt = Parameters::defaultOdomStrategy();
61  Parameters::parse(parameters, Parameters::kOdomStrategy(), odomTypeInt);
62  Odometry::Type type = (Odometry::Type)odomTypeInt;
63  return create(type, parameters);
64 }
65 
66 Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & parameters)
67 {
68  UDEBUG("type=%d", (int)type);
69  Odometry * odometry = 0;
70  switch(type)
71  {
72  case Odometry::kTypeF2M:
73  odometry = new OdometryF2M(parameters);
74  break;
75  case Odometry::kTypeF2F:
76  odometry = new OdometryF2F(parameters);
77  break;
78  case Odometry::kTypeFovis:
79  odometry = new OdometryFovis(parameters);
80  break;
81  case Odometry::kTypeViso2:
82  odometry = new OdometryViso2(parameters);
83  break;
84  case Odometry::kTypeDVO:
85  odometry = new OdometryDVO(parameters);
86  break;
87  case Odometry::kTypeORBSLAM:
88 #if defined(RTABMAP_ORB_SLAM) and RTABMAP_ORB_SLAM == 2
89  odometry = new OdometryORBSLAM2(parameters);
90 #else
91  odometry = new OdometryORBSLAM3(parameters);
92 #endif
93  break;
94  case Odometry::kTypeOkvis:
95  odometry = new OdometryOkvis(parameters);
96  break;
97  case Odometry::kTypeLOAM:
98  odometry = new OdometryLOAM(parameters);
99  break;
100  case Odometry::kTypeFLOAM:
101  odometry = new OdometryFLOAM(parameters);
102  break;
103  case Odometry::kTypeMSCKF:
104  odometry = new OdometryMSCKF(parameters);
105  break;
106  case Odometry::kTypeVINS:
107  odometry = new OdometryVINS(parameters);
108  break;
109  case Odometry::kTypeOpenVINS:
110  odometry = new OdometryOpenVINS(parameters);
111  break;
112  case Odometry::kTypeOpen3D:
113  odometry = new OdometryOpen3D(parameters);
114  break;
115  default:
116  UERROR("Unknown odometry type %d, using F2M instead...", (int)type);
117  odometry = new OdometryF2M(parameters);
118  type = Odometry::kTypeF2M;
119  break;
120  }
121  return odometry;
122 }
123 
124 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
125  _resetCountdown(Parameters::defaultOdomResetCountdown()),
126  _force3DoF(Parameters::defaultRegForce3DoF()),
127  _holonomic(Parameters::defaultOdomHolonomic()),
128  guessFromMotion_(Parameters::defaultOdomGuessMotion()),
129  guessSmoothingDelay_(Parameters::defaultOdomGuessSmoothingDelay()),
130  _filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
131  _particleSize(Parameters::defaultOdomParticleSize()),
132  _particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
133  _particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
134  _particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
135  _particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
136  _fillInfoData(Parameters::defaultOdomFillInfoData()),
137  _kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
138  _kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
139  _imageDecimation(Parameters::defaultOdomImageDecimation()),
140  _alignWithGround(Parameters::defaultOdomAlignWithGround()),
141  _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
142  _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
143  _deskewing(Parameters::defaultOdomDeskewing()),
144  _pose(Transform::getIdentity()),
145  _resetCurrentCount(0),
146  previousStamp_(0),
147  distanceTravelled_(0),
148  framesProcessed_(0)
149 {
150  Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
151 
152  Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
153  Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
154  Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
155  Parameters::parse(parameters, Parameters::kOdomGuessSmoothingDelay(), guessSmoothingDelay_);
156  Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
157  Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
158  Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
159  Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
160  Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
161  Parameters::parse(parameters, Parameters::kOdomParticleNoiseR(), _particleNoiseR);
162  Parameters::parse(parameters, Parameters::kOdomParticleLambdaR(), _particleLambdaR);
163  UASSERT(_particleNoiseT>0);
164  UASSERT(_particleLambdaT>0);
165  UASSERT(_particleNoiseR>0);
166  UASSERT(_particleLambdaR>0);
167  Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
168  Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
169  Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
170  Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
171  Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
172  Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
173  Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing);
174 
175  if(_imageDecimation == 0)
176  {
177  _imageDecimation = 1;
178  }
179 
180  if(_filteringStrategy == 2)
181  {
182  // Initialize the Particle filters
183  particleFilters_.resize(6);
184  for(unsigned int i = 0; i<particleFilters_.size(); ++i)
185  {
186  if(i<3)
187  {
188  particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
189  }
190  else
191  {
192  particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
193  }
194  }
195  }
196  else if(_filteringStrategy == 1)
197  {
198  initKalmanFilter();
199  }
200 }
201 
202 Odometry::~Odometry()
203 {
204  for(unsigned int i=0; i<particleFilters_.size(); ++i)
205  {
206  delete particleFilters_[i];
207  }
208 }
209 
210 void Odometry::reset(const Transform & initialPose)
211 {
212  UDEBUG("");
213  UASSERT(!initialPose.isNull());
214  previousVelocities_.clear();
215  velocityGuess_.setNull();
216  previousGroundTruthPose_.setNull();
217  _resetCurrentCount = 0;
218  previousStamp_ = 0;
219  distanceTravelled_ = 0;
220  framesProcessed_ = 0;
221  imuLastTransform_.setNull();
222  imus_.clear();
223  if(_force3DoF || particleFilters_.size())
224  {
225  float x,y,z, roll,pitch,yaw;
226  initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
227 
228  if(_force3DoF)
229  {
230  if(z != 0.0f || roll != 0.0f || pitch != 0.0f)
231  {
232  UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
233  }
234  z = 0;
235  roll = 0;
236  pitch = 0;
237  Transform pose(x, y, z, roll, pitch, yaw);
238  _pose = pose;
239  }
240  else
241  {
242  _pose = initialPose;
243  }
244 
245  if(particleFilters_.size())
246  {
247  UASSERT(particleFilters_.size() == 6);
248  particleFilters_[0]->init(x);
249  particleFilters_[1]->init(y);
250  particleFilters_[2]->init(z);
251  particleFilters_[3]->init(roll);
252  particleFilters_[4]->init(pitch);
253  particleFilters_[5]->init(yaw);
254  }
255 
256  if(_filteringStrategy == 1)
257  {
258  initKalmanFilter(initialPose);
259  }
260  }
261  else
262  {
263  _pose = initialPose;
264  }
265 }
266 
267 const Transform & Odometry::previousVelocityTransform() const
268 {
269  return getVelocityGuess();
270 }
271 
272 Transform getMeanVelocity(const std::list<std::pair<std::vector<float>, double> > & transforms)
273 {
274  if(transforms.size())
275  {
276  float tvx=0.0f,tvy=0.0f,tvz=0.0f, tvroll=0.0f,tvpitch=0.0f,tvyaw=0.0f;
277  for(std::list<std::pair<std::vector<float>, double> >::const_iterator iter=transforms.begin(); iter!=transforms.end(); ++iter)
278  {
279  UASSERT(iter->first.size() == 6);
280  tvx+=iter->first[0];
281  tvy+=iter->first[1];
282  tvz+=iter->first[2];
283  tvroll+=iter->first[3];
284  tvpitch+=iter->first[4];
285  tvyaw+=iter->first[5];
286  }
287  tvx/=float(transforms.size());
288  tvy/=float(transforms.size());
289  tvz/=float(transforms.size());
290  tvroll/=float(transforms.size());
291  tvpitch/=float(transforms.size());
292  tvyaw/=float(transforms.size());
293  return Transform(tvx, tvy, tvz, tvroll, tvpitch, tvyaw);
294  }
295  return Transform();
296 }
297 
298 Transform Odometry::process(SensorData & data, OdometryInfo * info)
299 {
300  return process(data, Transform(), info);
301 }
302 
303 Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
304 {
305  UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
306 
307  // cache imu data
308  if(!data.imu().empty() && !this->canProcessAsyncIMU())
309  {
310  if(!(data.imu().orientation()[0] == 0.0 && data.imu().orientation()[1] == 0.0 && data.imu().orientation()[2] == 0.0))
311  {
312  Transform orientation(0,0,0, data.imu().orientation()[0], data.imu().orientation()[1], data.imu().orientation()[2], data.imu().orientation()[3]);
313  // orientation includes roll and pitch but not yaw in local transform
314  Transform imuT = Transform(data.imu().localTransform().x(),data.imu().localTransform().y(),data.imu().localTransform().z(), 0,0,data.imu().localTransform().theta()) *
315  orientation*
316  data.imu().localTransform().rotation().inverse();
317 
318  if( this->getPose().r11() == 1.0f && this->getPose().r22() == 1.0f && this->getPose().r33() == 1.0f &&
319  this->framesProcessed() == 0)
320  {
321  Eigen::Quaterniond imuQuat = imuT.getQuaterniond();
322  Transform previous = this->getPose();
323  Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
324  UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
325  this->reset(newFramePose);
326  }
327 
328  imus_.insert(std::make_pair(data.stamp(), imuT));
329  if(imus_.size() > 1000)
330  {
331  imus_.erase(imus_.begin());
332  }
333  }
334  else
335  {
336  UWARN("Received IMU doesn't have orientation set! It is ignored.");
337  }
338  }
339 
340  if((data.imageRaw().empty() && !data.imageCompressed().empty()) ||
341  (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()) ||
342  (data.laserScanRaw().empty() && !data.laserScanCompressed().empty()))
343  {
344  UDEBUG("Received compressed data, uncompressing...");
345  data.uncompressData();
346  UDEBUG("Received compressed data, uncompressing...done!");
347  }
348 
349  if(!data.imageRaw().empty())
350  {
351  UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",
352  data.imageRaw().cols,
353  data.imageRaw().rows,
354  data.cameraModels().size(),
355  data.stereoCameraModels().size());
356  }
357 
358 
359  if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
360  {
361  if(!data.stereoCameraModels().empty())
362  {
363  bool valid = true;
364  if(data.stereoCameraModels().size() != stereoModels_.size())
365  {
366  stereoModels_.clear();
367  valid = false;
368  }
369  else
370  {
371  for(size_t i=0; i<data.stereoCameraModels().size() && valid; ++i)
372  {
373  valid = stereoModels_[i].isRectificationMapInitialized() &&
374  stereoModels_[i].left().imageSize() == data.stereoCameraModels()[i].left().imageSize();
375  }
376  }
377 
378  if(!valid)
379  {
380  stereoModels_ = data.stereoCameraModels();
381  valid = true;
382  for(size_t i=0; i<stereoModels_.size() && valid; ++i)
383  {
384  stereoModels_[i].initRectificationMap();
385  valid = stereoModels_[i].isRectificationMapInitialized();
386  }
387  if(valid)
388  {
389  UWARN("%s parameter is set to false but the selected odometry approach cannot "
390  "process raw stereo images. We will rectify them for convenience.",
391  Parameters::kRtabmapImagesAlreadyRectified().c_str());
392  }
393  else
394  {
395  UERROR("Odometry approach chosen cannot process raw stereo images (not rectified images) "
396  "and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
397  "Make sure images are rectified and set %s parameter back to true, or "
398  "make sure calibration is valid for rectification",
399  Parameters::kRtabmapImagesAlreadyRectified().c_str());
400  stereoModels_.clear();
401  }
402  }
403  if(valid)
404  {
405  if(stereoModels_.size()==1)
406  {
407  data.setStereoImage(
408  stereoModels_[0].left().rectifyImage(data.imageRaw()),
409  stereoModels_[0].right().rectifyImage(data.rightRaw()),
410  stereoModels_,
411  false);
412  }
413  else
414  {
415  UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
416  int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
417  cv::Mat rectifiedLeftImages = data.imageRaw().clone();
418  cv::Mat rectifiedRightImages = data.imageRaw().clone();
419  for(size_t i=0; i<stereoModels_.size() && valid; ++i)
420  {
421  cv::Mat rectifiedLeft = stereoModels_[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
422  cv::Mat rectifiedRight = stereoModels_[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
423  rectifiedLeft.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
424  rectifiedRight.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
425  }
426  data.setStereoImage(rectifiedLeftImages, rectifiedRightImages, stereoModels_, false);
427  }
428  }
429  }
430  else if(!data.cameraModels().empty())
431  {
432  bool valid = true;
433  if(data.cameraModels().size() != models_.size())
434  {
435  models_.clear();
436  valid = false;
437  }
438  else
439  {
440  for(size_t i=0; i<data.cameraModels().size() && valid; ++i)
441  {
442  valid = models_[i].isRectificationMapInitialized() &&
443  models_[i].imageSize() == data.cameraModels()[i].imageSize();
444  }
445  }
446 
447  if(!valid)
448  {
449  models_ = data.cameraModels();
450  valid = true;
451  for(size_t i=0; i<models_.size() && valid; ++i)
452  {
453  valid = models_[i].initRectificationMap();
454  }
455  if(valid)
456  {
457  UWARN("%s parameter is set to false but the selected odometry approach cannot "
458  "process raw images. We will rectify them for convenience (only "
459  "rgb is rectified, we assume depth image is already rectified!).",
460  Parameters::kRtabmapImagesAlreadyRectified().c_str());
461  }
462  else
463  {
464  UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
465  "and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
466  "Make sure images are rectified and set %s parameter back to true, or "
467  "make sure calibration is valid for rectification",
468  Parameters::kRtabmapImagesAlreadyRectified().c_str());
469  models_.clear();
470  }
471  }
472  if(valid)
473  {
474  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
475  if(models_.size()==1)
476  {
477  data.setRGBDImage(models_[0].rectifyImage(data.imageRaw()), data.depthRaw(), models_, false);
478  }
479  else
480  {
481  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
482  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
483  cv::Mat rectifiedImages = data.imageRaw().clone();
484  for(size_t i=0; i<models_.size() && valid; ++i)
485  {
486  cv::Mat rectifiedImage = models_[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
487  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
488  }
489  data.setRGBDImage(rectifiedImages, data.depthRaw(), models_, false);
490  }
491  }
492  }
493  else
494  {
495  UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
496  "are rectified, and set %s parameter back to true, or make sure that calibration is valid "
497  "for rectification so we can rectifiy them for convenience",
498  Parameters::kRtabmapImagesAlreadyRectified().c_str());
499  }
500  }
501 
502  // Ground alignment
503  if(_pose.x() == 0 && _pose.y() == 0 && _pose.z() == 0 && this->framesProcessed() == 0 && _alignWithGround)
504  {
505  if(data.depthOrRightRaw().empty())
506  {
507  UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
508  }
509  else
510  {
511  UTimer alignTimer;
512  pcl::IndicesPtr indices(new std::vector<int>);
513  pcl::IndicesPtr ground, obstacles;
514  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
515  bool success = false;
516  if(indices->size())
517  {
518  cloud = util3d::voxelize(cloud, indices, 0.01);
519  if(!_pose.isIdentity())
520  {
521  // In case we are already aligned with gravity
522  cloud = util3d::transformPointCloud(cloud, _pose);
523  }
524  util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
525  if(ground->size())
526  {
527  pcl::ModelCoefficients coefficients;
528  util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
529  if(coefficients.values.at(3) >= 0)
530  {
531  UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
532  coefficients.values.at(0),
533  coefficients.values.at(1),
534  coefficients.values.at(2),
535  coefficients.values.at(3),
536  alignTimer.ticks());
537  }
538  else
539  {
540  UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
541  coefficients.values.at(0),
542  coefficients.values.at(1),
543  coefficients.values.at(2),
544  coefficients.values.at(3),
545  alignTimer.ticks());
546  }
547  Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
548  Eigen::Vector3f z(0,0,1);
549  //get rotation from z to n;
550  Eigen::Matrix3f R;
551  R = Eigen::Quaternionf().setFromTwoVectors(n,z);
552  if(_pose.r11() == 1.0f && _pose.r22() == 1.0f && _pose.r33() == 1.0f)
553  {
554  Transform rotation(
555  R(0,0), R(0,1), R(0,2), 0,
556  R(1,0), R(1,1), R(1,2), 0,
557  R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
558  this->reset(rotation);
559  }
560  else
561  {
562  // Rotation is already set (e.g., from IMU/gravity), just update Z
563  UWARN("Rotation was already initialized, just offseting z to %f", coefficients.values.at(3));
564  Transform pose = _pose;
565  pose.z() = coefficients.values.at(3);
566  this->reset(pose);
567  }
568  success = true;
569  }
570  }
571  if(!success)
572  {
573  UERROR("Odometry failed to detect the ground. You have this "
574  "error because parameter \"%s\" is true. "
575  "Make sure the camera is seeing the ground (e.g., tilt ~30 "
576  "degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
577  }
578  }
579  }
580 
581  // KITTI datasets start with stamp=0
582  double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
583  Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
584  if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
585  {
586  if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
587  {
588  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());
589  }
590  else if(_filteringStrategy==1)
591  {
592  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());
593  }
594  dt=0;
595  previousVelocities_.clear();
596  velocityGuess_.setNull();
597  }
598  if(!velocityGuess_.isNull())
599  {
600  if(guessFromMotion_)
601  {
602  if(_filteringStrategy == 1)
603  {
604  // use Kalman predict transform
605  float vx,vy,vz, vroll,vpitch,vyaw;
606  predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
607  guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
608  }
609  else
610  {
611  float vx,vy,vz, vroll,vpitch,vyaw;
612  velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
613  guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
614  }
615  }
616  else if(_filteringStrategy == 1)
617  {
618  predictKalmanFilter(dt);
619  }
620  }
621 
622  Transform imuCurrentTransform;
623  if(!guessIn.isNull())
624  {
625  guess = guessIn;
626  }
627  else if(!imus_.empty())
628  {
629  // replace orientation guess with IMU (if available)
630  imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
631  if(!imuCurrentTransform.isNull() && !imuLastTransform_.isNull())
632  {
633  Transform orientation = imuLastTransform_.inverse() * imuCurrentTransform;
634  guess = Transform(
635  orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
636  orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
637  orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
638  if(_force3DoF)
639  {
640  guess = guess.to3DoF();
641  }
642  }
643  else if(!imuLastTransform_.isNull())
644  {
645  UWARN("Could not find imu transform at %f", data.stamp());
646  }
647  }
648 
649  UTimer time;
650 
651  // Deskewing lidar
652  if( _deskewing &&
653  !data.laserScanRaw().empty() &&
654  data.laserScanRaw().hasTime() &&
655  dt > 0 &&
656  !guess.isNull())
657  {
658  UDEBUG("Deskewing begin");
659  // Recompute velocity
660  float vx,vy,vz, vroll,vpitch,vyaw;
661  guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
662 
663  // transform to velocity
664  vx /= dt;
665  vy /= dt;
666  vz /= dt;
667  vroll /= dt;
668  vpitch /= dt;
669  vyaw /= dt;
670 
671  if(!imus_.empty())
672  {
673  float scanTime =
674  data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
675  data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
676 
677  // replace orientation velocity based on IMU (if available)
678  Transform imuFirstScan = Transform::getTransform(imus_,
679  data.stamp() +
680  data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
681  Transform imuLastScan = Transform::getTransform(imus_,
682  data.stamp() +
683  data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
684  if(!imuFirstScan.isNull() && !imuLastScan.isNull())
685  {
686  Transform orientation = imuFirstScan.inverse() * imuLastScan;
687  orientation.getEulerAngles(vroll, vpitch, vyaw);
688  if(_force3DoF)
689  {
690  vroll=0;
691  vpitch=0;
692  vyaw /= scanTime;
693  }
694  else
695  {
696  vroll /= scanTime;
697  vpitch /= scanTime;
698  vyaw /= scanTime;
699  }
700  }
701  }
702 
703  Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
704  LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
705  if(!scanDeskewed.isEmpty())
706  {
707  data.setLaserScan(scanDeskewed);
708  }
709  info->timeDeskewing = time.ticks();
710  UDEBUG("Deskewing end");
711  }
712  if(data.laserScanRaw().isOrganized())
713  {
714  // Laser scans should be dense passing this point
715  data.setLaserScan(data.laserScanRaw().densify());
716  }
717 
718 
719  Transform t;
720  if(_imageDecimation > 1 && !data.imageRaw().empty())
721  {
722  // Decimation of images with calibrations
723  SensorData decimatedData = data;
724  int decimationDepth = _imageDecimation;
725  if( !data.cameraModels().empty() &&
726  data.cameraModels()[0].imageHeight()>0 &&
727  data.cameraModels()[0].imageWidth()>0)
728  {
729  // decimate from RGB image size
730  int targetSize = data.cameraModels()[0].imageHeight() / _imageDecimation;
731  if(targetSize >= data.depthRaw().rows)
732  {
733  decimationDepth = 1;
734  }
735  else
736  {
737  decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
738  }
739  }
740  UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imageDecimation, data.depthOrRightRaw().rows, decimationDepth);
741 
742  cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), _imageDecimation);
743  cv::Mat depthRight = util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth);
744  std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
745  for(unsigned int i=0; i<cameraModels.size(); ++i)
746  {
747  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
748  }
749  if(!cameraModels.empty())
750  {
751  decimatedData.setRGBDImage(rgbLeft, depthRight, cameraModels);
752  }
753  else
754  {
755  std::vector<StereoCameraModel> stereoModels = decimatedData.stereoCameraModels();
756  for(unsigned int i=0; i<stereoModels.size(); ++i)
757  {
758  stereoModels[i].scale(1.0/double(_imageDecimation));
759  }
760  if(!stereoModels.empty())
761  {
762  decimatedData.setStereoImage(rgbLeft, depthRight, stereoModels);
763  }
764  }
765 
766 
767  // compute transform
768  t = this->computeTransform(decimatedData, guess, info);
769 
770  // transform back the keypoints in the original image
771  std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
772  double log2value = log(double(_imageDecimation))/log(2.0);
773  for(unsigned int i=0; i<kpts.size(); ++i)
774  {
775  kpts[i].pt.x *= _imageDecimation;
776  kpts[i].pt.y *= _imageDecimation;
777  kpts[i].size *= _imageDecimation;
778  kpts[i].octave += log2value;
779  }
780  data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
781  data.setLaserScan(decimatedData.laserScanRaw());
782 
783  if(info)
784  {
785  UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty());
786  for(unsigned int i=0; i<info->newCorners.size(); ++i)
787  {
788  info->newCorners[i].x *= _imageDecimation;
789  info->newCorners[i].y *= _imageDecimation;
790  if(!info->refCorners.empty())
791  {
792  info->refCorners[i].x *= _imageDecimation;
793  info->refCorners[i].y *= _imageDecimation;
794  }
795  }
796  for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
797  {
798  iter->second.pt.x *= _imageDecimation;
799  iter->second.pt.y *= _imageDecimation;
800  iter->second.size *= _imageDecimation;
801  iter->second.octave += log2value;
802  }
803  }
804  }
805  else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
806  {
807  t = this->computeTransform(data, guess, info);
808  }
809 
810  if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
811  {
812  return Transform(); // Return null on IMU-only updates
813  }
814 
815  if(info)
816  {
817  info->timeEstimation = time.ticks();
818  info->lost = t.isNull();
819  info->stamp = data.stamp();
820  info->interval = dt;
821  info->transform = t;
822  info->guess = guess;
823  if(_publishRAMUsage)
824  {
825  info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
826  }
827 
828  if(!data.groundTruth().isNull())
829  {
830  if(!previousGroundTruthPose_.isNull())
831  {
832  info->transformGroundTruth = previousGroundTruthPose_.inverse() * data.groundTruth();
833  }
834  previousGroundTruthPose_ = data.groundTruth();
835  }
836  }
837 
838  if(!t.isNull())
839  {
840  _resetCurrentCount = _resetCountdown;
841 
842  float vx,vy,vz, vroll,vpitch,vyaw;
843  t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
844 
845  // transform to velocity
846  if(dt)
847  {
848  vx /= dt;
849  vy /= dt;
850  vz /= dt;
851  vroll /= dt;
852  vpitch /= dt;
853  vyaw /= dt;
854  }
855 
856  if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
857  {
858  if(_filteringStrategy == 1)
859  {
860  if(velocityGuess_.isNull())
861  {
862  // reset Kalman
863  if(dt)
864  {
865  initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
866  }
867  else
868  {
869  initKalmanFilter(t);
870  }
871  }
872  else
873  {
874  // Kalman filtering
875  updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
876  }
877  }
878  else
879  {
880  if(particleFilters_.size())
881  {
882  // Particle filtering
883  UASSERT(particleFilters_.size()==6);
884  if(velocityGuess_.isNull())
885  {
886  particleFilters_[0]->init(vx);
887  particleFilters_[1]->init(vy);
888  particleFilters_[2]->init(vz);
889  particleFilters_[3]->init(vroll);
890  particleFilters_[4]->init(vpitch);
891  particleFilters_[5]->init(vyaw);
892  }
893  else
894  {
895  vx = particleFilters_[0]->filter(vx);
896  vy = particleFilters_[1]->filter(vy);
897  vyaw = particleFilters_[5]->filter(vyaw);
898 
899  if(!_holonomic)
900  {
901  // arc trajectory around ICR
902  float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
903  if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
904  {
905  vy = tmpY;
906  }
907  else
908  {
909  vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
910  }
911  }
912 
913  if(!_force3DoF)
914  {
915  vz = particleFilters_[2]->filter(vz);
916  vroll = particleFilters_[3]->filter(vroll);
917  vpitch = particleFilters_[4]->filter(vpitch);
918  }
919  }
920 
921  if(info)
922  {
923  info->timeParticleFiltering = time.ticks();
924  }
925  }
926  else if(!_holonomic)
927  {
928  // arc trajectory around ICR
929  vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
930  }
931 
932  if(_force3DoF)
933  {
934  vz = 0.0f;
935  vroll = 0.0f;
936  vpitch = 0.0f;
937  }
938  }
939 
940  if(dt)
941  {
942  t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
943  }
944  else
945  {
946  t = Transform(vx, vy, vz, vroll, vpitch, vyaw);
947  }
948 
949  if(info)
950  {
951  info->transformFiltered = t;
952  }
953  }
954 
955  if(data.stamp() == 0 && framesProcessed_ != 0)
956  {
957  UWARN("Null stamp detected");
958  }
959 
960  previousStamp_ = data.stamp();
961 
962  if(dt)
963  {
964  if(dt >= (guessSmoothingDelay_/2.0) || particleFilters_.size() || _filteringStrategy==1)
965  {
966  velocityGuess_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
967  previousVelocities_.clear();
968  }
969  else
970  {
971  // smooth velocity estimation over the past X seconds
972  std::vector<float> v(6);
973  v[0] = vx;
974  v[1] = vy;
975  v[2] = vz;
976  v[3] = vroll;
977  v[4] = vpitch;
978  v[5] = vyaw;
979  previousVelocities_.push_back(std::make_pair(v, data.stamp()));
980  while(previousVelocities_.size() > 1 && previousVelocities_.front().second < previousVelocities_.back().second-guessSmoothingDelay_)
981  {
982  previousVelocities_.pop_front();
983  }
984  velocityGuess_ = getMeanVelocity(previousVelocities_);
985  }
986  }
987  else
988  {
989  previousVelocities_.clear();
990  velocityGuess_.setNull();
991  }
992 
993  if(info)
994  {
995  distanceTravelled_ += t.getNorm();
996  info->distanceTravelled = distanceTravelled_;
997  info->guessVelocity = velocityGuess_;
998  }
999  ++framesProcessed_;
1000 
1001  imuLastTransform_ = imuCurrentTransform;
1002 
1003  return _pose *= t; // update
1004  }
1005  else if(_resetCurrentCount > 0)
1006  {
1007  UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
1008 
1009  --_resetCurrentCount;
1010  if(_resetCurrentCount == 0)
1011  {
1012  UWARN("Odometry automatically reset to latest pose!");
1013  this->reset(_pose);
1014  _resetCurrentCount = _resetCountdown;
1015  if(info)
1016  {
1017  *info = OdometryInfo();
1018  }
1019  return this->computeTransform(data, Transform(), info);
1020  }
1021  }
1022 
1023  previousVelocities_.clear();
1024  velocityGuess_.setNull();
1025  previousStamp_ = 0;
1026 
1027  return Transform();
1028 }
1029 
1030 void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
1031 {
1032  UDEBUG("");
1033  // See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
1034  // See Kalman filter pose/orientation estimation theory: http://campar.in.tum.de/Chair/KalmanFilter
1035 
1036  // initialize the Kalman filter
1037  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'')
1038  int nMeasurements = 6; // the number of measured states (x',y',z',roll',pitch',yaw')
1039  if(_force3DoF)
1040  {
1041  nStates = 9; // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
1042  nMeasurements = 3; // the number of measured states (x',y',yaw')
1043  }
1044  int nInputs = 0; // the number of action control
1045 
1046  /* From viso2, measurement covariance
1047  * static const boost::array<double, 36> STANDARD_POSE_COVARIANCE =
1048  { { 0.1, 0, 0, 0, 0, 0,
1049  0, 0.1, 0, 0, 0, 0,
1050  0, 0, 0.1, 0, 0, 0,
1051  0, 0, 0, 0.17, 0, 0,
1052  0, 0, 0, 0, 0.17, 0,
1053  0, 0, 0, 0, 0, 0.17 } };
1054  static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
1055  { { 0.05, 0, 0, 0, 0, 0,
1056  0, 0.05, 0, 0, 0, 0,
1057  0, 0, 0.05, 0, 0, 0,
1058  0, 0, 0, 0.09, 0, 0,
1059  0, 0, 0, 0, 0.09, 0,
1060  0, 0, 0, 0, 0, 0.09 } };
1061  */
1062 
1063 
1064  kalmanFilter_.init(nStates, nMeasurements, nInputs); // init Kalman Filter
1065  cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise)); // set process noise
1066  cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise)); // set measurement noise
1067  cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1)); // error covariance
1068 
1069  float x,y,z,roll,pitch,yaw;
1070  initialPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1071 
1072  if(_force3DoF)
1073  {
1074  /* MEASUREMENT MODEL (velocity) */
1075  // [0 0 1 0 0 0 0 0 0]
1076  // [0 0 0 1 0 0 0 0 0]
1077  // [0 0 0 0 0 0 0 1 0]
1078  kalmanFilter_.measurementMatrix.at<float>(0,2) = 1; // x'
1079  kalmanFilter_.measurementMatrix.at<float>(1,3) = 1; // y'
1080  kalmanFilter_.measurementMatrix.at<float>(2,7) = 1; // yaw'
1081 
1082  kalmanFilter_.statePost.at<float>(0) = x;
1083  kalmanFilter_.statePost.at<float>(1) = y;
1084  kalmanFilter_.statePost.at<float>(6) = yaw;
1085 
1086  kalmanFilter_.statePost.at<float>(2) = vx;
1087  kalmanFilter_.statePost.at<float>(3) = vy;
1088  kalmanFilter_.statePost.at<float>(7) = vyaw;
1089  }
1090  else
1091  {
1092  /* MEASUREMENT MODEL (velocity) */
1093  // [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
1094  // [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0]
1095  // [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]
1096  // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0]
1097  // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0]
1098  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0]
1099  kalmanFilter_.measurementMatrix.at<float>(0,3) = 1; // x'
1100  kalmanFilter_.measurementMatrix.at<float>(1,4) = 1; // y'
1101  kalmanFilter_.measurementMatrix.at<float>(2,5) = 1; // z'
1102  kalmanFilter_.measurementMatrix.at<float>(3,12) = 1; // roll'
1103  kalmanFilter_.measurementMatrix.at<float>(4,13) = 1; // pitch'
1104  kalmanFilter_.measurementMatrix.at<float>(5,14) = 1; // yaw'
1105 
1106  kalmanFilter_.statePost.at<float>(0) = x;
1107  kalmanFilter_.statePost.at<float>(1) = y;
1108  kalmanFilter_.statePost.at<float>(2) = z;
1109  kalmanFilter_.statePost.at<float>(9) = roll;
1110  kalmanFilter_.statePost.at<float>(10) = pitch;
1111  kalmanFilter_.statePost.at<float>(11) = yaw;
1112 
1113  kalmanFilter_.statePost.at<float>(3) = vx;
1114  kalmanFilter_.statePost.at<float>(4) = vy;
1115  kalmanFilter_.statePost.at<float>(5) = vz;
1116  kalmanFilter_.statePost.at<float>(12) = vroll;
1117  kalmanFilter_.statePost.at<float>(13) = vpitch;
1118  kalmanFilter_.statePost.at<float>(14) = vyaw;
1119  }
1120 }
1121 
1122 void Odometry::predictKalmanFilter(float dt, float * vx, float * vy, float * vz, float * vroll, float * vpitch, float * vyaw)
1123 {
1124  // Set transition matrix with current dt
1125  if(_force3DoF)
1126  {
1127  // 2D:
1128  // [1 0 dt 0 dt2 0 0 0 0] x
1129  // [0 1 0 dt 0 dt2 0 0 0] y
1130  // [0 0 1 0 dt 0 0 0 0] x'
1131  // [0 0 0 1 0 dt 0 0 0] y'
1132  // [0 0 0 0 1 0 0 0 0] x''
1133  // [0 0 0 0 0 0 0 0 0] y''
1134  // [0 0 0 0 0 0 1 dt dt2] yaw
1135  // [0 0 0 0 0 0 0 1 dt] yaw'
1136  // [0 0 0 0 0 0 0 0 1] yaw''
1137  kalmanFilter_.transitionMatrix.at<float>(0,2) = dt;
1138  kalmanFilter_.transitionMatrix.at<float>(1,3) = dt;
1139  kalmanFilter_.transitionMatrix.at<float>(2,4) = dt;
1140  kalmanFilter_.transitionMatrix.at<float>(3,5) = dt;
1141  kalmanFilter_.transitionMatrix.at<float>(0,4) = 0.5*pow(dt,2);
1142  kalmanFilter_.transitionMatrix.at<float>(1,5) = 0.5*pow(dt,2);
1143  // orientation
1144  kalmanFilter_.transitionMatrix.at<float>(6,7) = dt;
1145  kalmanFilter_.transitionMatrix.at<float>(7,8) = dt;
1146  kalmanFilter_.transitionMatrix.at<float>(6,8) = 0.5*pow(dt,2);
1147  }
1148  else
1149  {
1150  // [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0] x
1151  // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] y
1152  // [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0] z
1153  // [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] x'
1154  // [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0] y'
1155  // [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] z'
1156  // [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0] x''
1157  // [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0] y''
1158  // [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0] z''
1159  // [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
1160  // [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
1161  // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
1162  // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
1163  // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
1164  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
1165  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
1166  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
1167  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
1168  // position
1169  kalmanFilter_.transitionMatrix.at<float>(0,3) = dt;
1170  kalmanFilter_.transitionMatrix.at<float>(1,4) = dt;
1171  kalmanFilter_.transitionMatrix.at<float>(2,5) = dt;
1172  kalmanFilter_.transitionMatrix.at<float>(3,6) = dt;
1173  kalmanFilter_.transitionMatrix.at<float>(4,7) = dt;
1174  kalmanFilter_.transitionMatrix.at<float>(5,8) = dt;
1175  kalmanFilter_.transitionMatrix.at<float>(0,6) = 0.5*pow(dt,2);
1176  kalmanFilter_.transitionMatrix.at<float>(1,7) = 0.5*pow(dt,2);
1177  kalmanFilter_.transitionMatrix.at<float>(2,8) = 0.5*pow(dt,2);
1178  // orientation
1179  kalmanFilter_.transitionMatrix.at<float>(9,12) = dt;
1180  kalmanFilter_.transitionMatrix.at<float>(10,13) = dt;
1181  kalmanFilter_.transitionMatrix.at<float>(11,14) = dt;
1182  kalmanFilter_.transitionMatrix.at<float>(12,15) = dt;
1183  kalmanFilter_.transitionMatrix.at<float>(13,16) = dt;
1184  kalmanFilter_.transitionMatrix.at<float>(14,17) = dt;
1185  kalmanFilter_.transitionMatrix.at<float>(9,15) = 0.5*pow(dt,2);
1186  kalmanFilter_.transitionMatrix.at<float>(10,16) = 0.5*pow(dt,2);
1187  kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
1188  }
1189 
1190  // First predict, to update the internal statePre variable
1191  UDEBUG("Predict");
1192  const cv::Mat & prediction = kalmanFilter_.predict();
1193 
1194  if(vx)
1195  *vx = prediction.at<float>(3); // x'
1196  if(vy)
1197  *vy = prediction.at<float>(4); // y'
1198  if(vz)
1199  *vz = _force3DoF?0.0f:prediction.at<float>(5); // z'
1200  if(vroll)
1201  *vroll = _force3DoF?0.0f:prediction.at<float>(12); // roll'
1202  if(vpitch)
1203  *vpitch = _force3DoF?0.0f:prediction.at<float>(13); // pitch'
1204  if(vyaw)
1205  *vyaw = prediction.at<float>(_force3DoF?7:14); // yaw'
1206 }
1207 
1208 void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
1209 {
1210  // Set measurement to predict
1211  cv::Mat measurements;
1212  if(!_force3DoF)
1213  {
1214  measurements = cv::Mat(6,1,CV_32FC1);
1215  measurements.at<float>(0) = vx; // x'
1216  measurements.at<float>(1) = vy; // y'
1217  measurements.at<float>(2) = vz; // z'
1218  measurements.at<float>(3) = vroll; // roll'
1219  measurements.at<float>(4) = vpitch; // pitch'
1220  measurements.at<float>(5) = vyaw; // yaw'
1221  }
1222  else
1223  {
1224  measurements = cv::Mat(3,1,CV_32FC1);
1225  measurements.at<float>(0) = vx; // x'
1226  measurements.at<float>(1) = vy; // y'
1227  measurements.at<float>(2) = vyaw; // yaw',
1228  }
1229 
1230  // The "correct" phase that is going to use the predicted value and our measurement
1231  UDEBUG("Correct");
1232  const cv::Mat & estimated = kalmanFilter_.correct(measurements);
1233 
1234 
1235  vx = estimated.at<float>(3); // x'
1236  vy = estimated.at<float>(4); // y'
1237  vz = _force3DoF?0.0f:estimated.at<float>(5); // z'
1238  vroll = _force3DoF?0.0f:estimated.at<float>(12); // roll'
1239  vpitch = _force3DoF?0.0f:estimated.at<float>(13); // pitch'
1240  vyaw = estimated.at<float>(_force3DoF?7:14); // yaw'
1241 }
1242 
1243 } /* namespace rtabmap */
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::Odometry::_resetCurrentCount
int _resetCurrentCount
Definition: Odometry.h:116
int
int
rtabmap::OdometryF2F
Definition: OdometryF2F.h:38
OdometryOkvis.h
rtabmap::Odometry::create
static Odometry * create(const ParametersMap &parameters=ParametersMap())
Definition: Odometry.cpp:58
glm::tan
GLM_FUNC_DECL genType tan(genType const &angle)
rtabmap::Odometry::_particleSize
int _particleSize
Definition: Odometry.h:102
rtabmap::Odometry::kTypeDVO
@ kTypeDVO
Definition: Odometry.h:51
rtabmap::OdometryFLOAM
Definition: OdometryFLOAM.h:38
OdometryORBSLAM3.h
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
OdometryInfo.h
rtabmap::Odometry::guessFromMotion_
bool guessFromMotion_
Definition: Odometry.h:99
rtabmap::Odometry::previousStamp_
double previousStamp_
Definition: Odometry.h:117
rtabmap::OdometryMSCKF
Definition: OdometryMSCKF.h:38
OdometryMSCKF.h
rtabmap::Odometry::_publishRAMUsage
bool _publishRAMUsage
Definition: Odometry.h:112
OdometryFovis.h
rtabmap::Transform::r33
float r33() const
Definition: Transform.h:70
OdometryOpenVINS.h
rtabmap::Odometry::updateKalmanFilter
void updateKalmanFilter(float &vx, float &vy, float &vz, float &vroll, float &vpitch, float &vyaw)
Definition: Odometry.cpp:1208
UProcessInfo.h
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
this
this
rtabmap::OdometryVINS
Definition: OdometryVINS.h:37
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:761
dt
const double dt
OdometryORBSLAM2.h
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:1030
rtabmap::Odometry::kTypeLOAM
@ kTypeLOAM
Definition: Odometry.h:54
OdometryDVO.h
type
OdometryViso2.h
rtabmap::Odometry::previousVelocities_
std::list< std::pair< std::vector< float >, double > > previousVelocities_
Definition: Odometry.h:118
rtabmap::Odometry::_particleNoiseR
float _particleNoiseR
Definition: Odometry.h:105
rtabmap::Odometry::framesProcessed_
unsigned int framesProcessed_
Definition: Odometry.h:123
y
Matrix3f y
rtabmap::Odometry::kTypeMSCKF
@ kTypeMSCKF
Definition: Odometry.h:55
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:2506
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
util3d.h
rtabmap::LaserScan
Definition: LaserScan.h:37
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:1122
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::OdometryOpenVINS
Definition: OdometryOpenVINS.h:40
rtabmap::Odometry::kTypeOpenVINS
@ kTypeOpenVINS
Definition: Odometry.h:57
UTimer.h
OdometryOpen3D.h
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::Odometry::velocityGuess_
Transform velocityGuess_
Definition: Odometry.h:119
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
glm::pow
GLM_FUNC_DECL genType pow(genType const &base, genType const &exponent)
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:506
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
indices
indices
OdometryF2F.h
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:258
rtabmap::Odometry::kTypeFLOAM
@ kTypeFLOAM
Definition: Odometry.h:58
rtabmap::Odometry::imus_
std::map< double, Transform > imus_
Definition: Odometry.h:129
fabs
Real fabs(const Real &a)
rtabmap::Odometry::kTypeViso2
@ kTypeViso2
Definition: Odometry.h:50
odometry
Pose2 odometry(2.0, 0.0, 0.0)
n
int n
Odometry.h
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
rtabmap::SensorData::setRGBDImage
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:227
rtabmap::Odometry::imuLastTransform_
Transform imuLastTransform_
Definition: Odometry.h:120
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::Odometry::previousGroundTruthPose_
Transform previousGroundTruthPose_
Definition: Odometry.h:121
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::Odometry::stereoModels_
std::vector< StereoCameraModel > stereoModels_
Definition: Odometry.h:127
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
data
int data[]
rtabmap::Odometry::_force3DoF
bool _force3DoF
Definition: Odometry.h:97
rtabmap::Transform::getTransform
static Transform getTransform(const std::map< double, Transform > &tfBuffer, const double &stamp)
Definition: Transform.cpp:530
vy
StridedVectorType vy(make_vector(y, *n, std::abs(*incy)))
rtabmap::Odometry::computeTransform
virtual Transform computeTransform(SensorData &data, const Transform &guess=Transform(), OdometryInfo *info=0)=0
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:276
UConversion.h
Some conversion functions.
util3d_filtering.h
rtabmap::Odometry::previousVelocityTransform
const RTABMAP_DEPRECATED Transform & previousVelocityTransform() const
Definition: Odometry.cpp:267
rtabmap::util3d::deskew
LaserScan RTABMAP_CORE_EXPORT deskew(const LaserScan &input, double inputStamp, const rtabmap::Transform &velocity)
Lidar deskewing.
Definition: util3d.cpp:3682
rtabmap::Odometry::_holonomic
bool _holonomic
Definition: Odometry.h:98
all
static const Eigen::internal::all_t all
rtabmap::Odometry::kTypeFovis
@ kTypeFovis
Definition: Odometry.h:49
rtabmap::Odometry::process
Transform process(SensorData &data, OdometryInfo *info=0)
Definition: Odometry.cpp:298
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
glm::ceil
GLM_FUNC_DECL genType ceil(genType const &x)
rtabmap::Odometry::_resetCountdown
int _resetCountdown
Definition: Odometry.h:96
velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
Eigen::Quaternionf
Quaternion< float > Quaternionf
glm::orientation
GLM_FUNC_DECL detail::tmat4x4< T, P > orientation(detail::tvec3< T, P > const &Normal, detail::tvec3< T, P > const &Up)
info
else if n * info
rtabmap::Odometry::getVelocityGuess
const Transform & getVelocityGuess() const
Definition: Odometry.h:80
UASSERT
#define UASSERT(condition)
z
z
x
x
rtabmap::Odometry::_particleLambdaR
float _particleLambdaR
Definition: Odometry.h:106
rtabmap::Odometry::framesProcessed
unsigned int framesProcessed() const
Definition: Odometry.h:82
rtabmap::OdometryOkvis
Definition: OdometryOkvis.h:40
rtabmap::Parameters
Definition: Parameters.h:170
util3d_mapping.h
rtabmap::Odometry::_particleLambdaT
float _particleLambdaT
Definition: Odometry.h:104
rtabmap::getMeanVelocity
Transform getMeanVelocity(const std::list< std::pair< std::vector< float >, double > > &transforms)
Definition: Odometry.cpp:272
time
#define time
rtabmap::Odometry::_kalmanProcessNoise
float _kalmanProcessNoise
Definition: Odometry.h:108
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::OdometryF2M
Definition: OdometryF2M.h:44
rtabmap::OdometryORBSLAM2
Definition: OdometryORBSLAM2.h:38
rtabmap::OdometryDVO
Definition: OdometryDVO.h:43
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::Odometry::kTypeVINS
@ kTypeVINS
Definition: Odometry.h:56
measurements
std::vector< double > measurements
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
glm::log
GLM_FUNC_DECL genType log(genType const &x)
rtabmap::Odometry::kTypeOkvis
@ kTypeOkvis
Definition: Odometry.h:53
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
rtabmap::Odometry::particleFilters_
std::vector< ParticleFilter * > particleFilters_
Definition: Odometry.h:125
rtabmap::Transform::r22
float r22() const
Definition: Transform.h:66
rtabmap::Transform
Definition: Transform.h:41
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT 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:1060
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
util2d.h
OdometryFLOAM.h
rtabmap::Odometry::_deskewing
bool _deskewing
Definition: Odometry.h:114
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::OdometryFovis
Definition: OdometryFovis.h:43
rtabmap::Odometry::_alignWithGround
bool _alignWithGround
Definition: Odometry.h:111
rtabmap::Odometry::_kalmanMeasurementNoise
float _kalmanMeasurementNoise
Definition: Odometry.h:109
rtabmap::Odometry::distanceTravelled_
float distanceTravelled_
Definition: Odometry.h:122
iter
iterator iter(handle obj)
rtabmap::OdometryOpen3D
Definition: OdometryOpen3D.h:35
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Odometry::kTypeORBSLAM
@ kTypeORBSLAM
Definition: Odometry.h:52
c_str
const char * c_str(Args &&...args)
ParticleFilter.h
rtabmap::Odometry::Odometry
Odometry(const rtabmap::ParametersMap &parameters)
Definition: Odometry.cpp:124
rtabmap::Odometry::guessSmoothingDelay_
float guessSmoothingDelay_
Definition: Odometry.h:100
v
Array< int, Dynamic, 1 > v
rtabmap::OdometryViso2
Definition: OdometryViso2.h:37
OdometryLOAM.h
UDEBUG
#define UDEBUG(...)
rtabmap::Odometry::_fillInfoData
bool _fillInfoData
Definition: Odometry.h:107
rtabmap::ParticleFilter
Definition: ParticleFilter.h:115
rtabmap::Odometry
Definition: Odometry.h:42
OdometryF2M.h
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:41
float
float
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:274
rtabmap::OdometryLOAM
Definition: OdometryLOAM.h:43
rtabmap::Odometry::Type
Type
Definition: Odometry.h:45
rtabmap::Odometry::_particleNoiseT
float _particleNoiseT
Definition: Odometry.h:103
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::Transform::to3DoF
Transform to3DoF() const
Definition: Transform.cpp:210
rtabmap::Odometry::kTypeOpen3D
@ kTypeOpen3D
Definition: Odometry.h:59
rtabmap::SensorData::setStereoImage
void setStereoImage(const cv::Mat &left, const cv::Mat &right, const StereoCameraModel &stereoCameraModel, bool clearPreviousData=true)
Definition: SensorData.cpp:304
rtabmap::Odometry::kTypeF2F
@ kTypeF2F
Definition: Odometry.h:48
glm::atan
GLM_FUNC_DECL genType atan(genType const &y, genType const &x)
rtabmap::Odometry::models_
std::vector< CameraModel > models_
Definition: Odometry.h:128
rtabmap::Odometry::_imageDecimation
unsigned int _imageDecimation
Definition: Odometry.h:110
t
Point2 t(10, 10)
OdometryVINS.h
rtabmap::Odometry::_pose
Transform _pose
Definition: Odometry.h:115
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::Transform::getQuaterniond
Eigen::Quaterniond getQuaterniond() const
Definition: Transform.cpp:406
UERROR
#define UERROR(...)
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:275
UProcessInfo::getMemoryUsage
static long int getMemoryUsage()
Definition: UProcessInfo.cpp:38
rtabmap::Odometry::kTypeF2M
@ kTypeF2M
Definition: Odometry.h:47
i
int i
rtabmap::Odometry::_imagesAlreadyRectified
bool _imagesAlreadyRectified
Definition: Odometry.h:113
rtabmap::Odometry::~Odometry
virtual ~Odometry()
Definition: Odometry.cpp:202
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Odometry::kalmanFilter_
cv::KalmanFilter kalmanFilter_
Definition: Odometry.h:126
vx
StridedVectorType vx(make_vector(x, *n, std::abs(*incx)))
rtabmap::OdometryORBSLAM3
Definition: OdometryORBSLAM3.h:39
rtabmap::Odometry::_filteringStrategy
int _filteringStrategy
Definition: Odometry.h:101
rtabmap::Odometry::canProcessRawImages
virtual bool canProcessRawImages() const
Definition: Odometry.h:72
R
R
rtabmap::Transform::r11
float r11() const
Definition: Transform.h:62

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