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())
341  {
342  UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",
343  data.imageRaw().cols,
344  data.imageRaw().rows,
345  data.cameraModels().size(),
346  data.stereoCameraModels().size());
347  }
348 
349 
350  if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
351  {
352  if(!data.stereoCameraModels().empty())
353  {
354  bool valid = true;
355  if(data.stereoCameraModels().size() != stereoModels_.size())
356  {
357  stereoModels_.clear();
358  valid = false;
359  }
360  else
361  {
362  for(size_t i=0; i<data.stereoCameraModels().size() && valid; ++i)
363  {
364  valid = stereoModels_[i].isRectificationMapInitialized() &&
365  stereoModels_[i].left().imageSize() == data.stereoCameraModels()[i].left().imageSize();
366  }
367  }
368 
369  if(!valid)
370  {
371  stereoModels_ = data.stereoCameraModels();
372  valid = true;
373  for(size_t i=0; i<stereoModels_.size() && valid; ++i)
374  {
375  stereoModels_[i].initRectificationMap();
376  valid = stereoModels_[i].isRectificationMapInitialized();
377  }
378  if(valid)
379  {
380  UWARN("%s parameter is set to false but the selected odometry approach cannot "
381  "process raw stereo images. We will rectify them for convenience.",
382  Parameters::kRtabmapImagesAlreadyRectified().c_str());
383  }
384  else
385  {
386  UERROR("Odometry approach chosen cannot process raw stereo images (not rectified images) "
387  "and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
388  "Make sure images are rectified and set %s parameter back to true, or "
389  "make sure calibration is valid for rectification",
390  Parameters::kRtabmapImagesAlreadyRectified().c_str());
391  stereoModels_.clear();
392  }
393  }
394  if(valid)
395  {
396  if(stereoModels_.size()==1)
397  {
398  data.setStereoImage(
399  stereoModels_[0].left().rectifyImage(data.imageRaw()),
400  stereoModels_[0].right().rectifyImage(data.rightRaw()),
401  stereoModels_,
402  false);
403  }
404  else
405  {
406  UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
407  int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
408  cv::Mat rectifiedLeftImages = data.imageRaw().clone();
409  cv::Mat rectifiedRightImages = data.imageRaw().clone();
410  for(size_t i=0; i<stereoModels_.size() && valid; ++i)
411  {
412  cv::Mat rectifiedLeft = stereoModels_[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
413  cv::Mat rectifiedRight = stereoModels_[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
414  rectifiedLeft.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
415  rectifiedRight.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
416  }
417  data.setStereoImage(rectifiedLeftImages, rectifiedRightImages, stereoModels_, false);
418  }
419  }
420  }
421  else if(!data.cameraModels().empty())
422  {
423  bool valid = true;
424  if(data.cameraModels().size() != models_.size())
425  {
426  models_.clear();
427  valid = false;
428  }
429  else
430  {
431  for(size_t i=0; i<data.cameraModels().size() && valid; ++i)
432  {
433  valid = models_[i].isRectificationMapInitialized() &&
434  models_[i].imageSize() == data.cameraModels()[i].imageSize();
435  }
436  }
437 
438  if(!valid)
439  {
440  models_ = data.cameraModels();
441  valid = true;
442  for(size_t i=0; i<models_.size() && valid; ++i)
443  {
444  valid = models_[i].initRectificationMap();
445  }
446  if(valid)
447  {
448  UWARN("%s parameter is set to false but the selected odometry approach cannot "
449  "process raw images. We will rectify them for convenience (only "
450  "rgb is rectified, we assume depth image is already rectified!).",
451  Parameters::kRtabmapImagesAlreadyRectified().c_str());
452  }
453  else
454  {
455  UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
456  "and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
457  "Make sure images are rectified and set %s parameter back to true, or "
458  "make sure calibration is valid for rectification",
459  Parameters::kRtabmapImagesAlreadyRectified().c_str());
460  models_.clear();
461  }
462  }
463  if(valid)
464  {
465  // Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
466  if(models_.size()==1)
467  {
468  data.setRGBDImage(models_[0].rectifyImage(data.imageRaw()), data.depthRaw(), models_, false);
469  }
470  else
471  {
472  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
473  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
474  cv::Mat rectifiedImages = data.imageRaw().clone();
475  for(size_t i=0; i<models_.size() && valid; ++i)
476  {
477  cv::Mat rectifiedImage = models_[i].rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
478  rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
479  }
480  data.setRGBDImage(rectifiedImages, data.depthRaw(), models_, false);
481  }
482  }
483  }
484  else
485  {
486  UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
487  "are rectified, and set %s parameter back to true, or make sure that calibration is valid "
488  "for rectification so we can rectifiy them for convenience",
489  Parameters::kRtabmapImagesAlreadyRectified().c_str());
490  }
491  }
492 
493  // Ground alignment
494  if(_pose.x() == 0 && _pose.y() == 0 && _pose.z() == 0 && this->framesProcessed() == 0 && _alignWithGround)
495  {
496  if(data.depthOrRightRaw().empty())
497  {
498  UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
499  }
500  else
501  {
502  UTimer alignTimer;
503  pcl::IndicesPtr indices(new std::vector<int>);
504  pcl::IndicesPtr ground, obstacles;
505  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
506  bool success = false;
507  if(indices->size())
508  {
509  cloud = util3d::voxelize(cloud, indices, 0.01);
510  if(!_pose.isIdentity())
511  {
512  // In case we are already aligned with gravity
513  cloud = util3d::transformPointCloud(cloud, _pose);
514  }
515  util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
516  if(ground->size())
517  {
518  pcl::ModelCoefficients coefficients;
519  util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
520  if(coefficients.values.at(3) >= 0)
521  {
522  UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
523  coefficients.values.at(0),
524  coefficients.values.at(1),
525  coefficients.values.at(2),
526  coefficients.values.at(3),
527  alignTimer.ticks());
528  }
529  else
530  {
531  UWARN("Ceiling 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  Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
539  Eigen::Vector3f z(0,0,1);
540  //get rotation from z to n;
541  Eigen::Matrix3f R;
542  R = Eigen::Quaternionf().setFromTwoVectors(n,z);
543  if(_pose.r11() == 1.0f && _pose.r22() == 1.0f && _pose.r33() == 1.0f)
544  {
545  Transform rotation(
546  R(0,0), R(0,1), R(0,2), 0,
547  R(1,0), R(1,1), R(1,2), 0,
548  R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
549  this->reset(rotation);
550  }
551  else
552  {
553  // Rotation is already set (e.g., from IMU/gravity), just update Z
554  UWARN("Rotation was already initialized, just offseting z to %f", coefficients.values.at(3));
555  Transform pose = _pose;
556  pose.z() = coefficients.values.at(3);
557  this->reset(pose);
558  }
559  success = true;
560  }
561  }
562  if(!success)
563  {
564  UERROR("Odometry failed to detect the ground. You have this "
565  "error because parameter \"%s\" is true. "
566  "Make sure the camera is seeing the ground (e.g., tilt ~30 "
567  "degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
568  }
569  }
570  }
571 
572  // KITTI datasets start with stamp=0
573  double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
574  Transform guess = dt>0.0 && guessFromMotion_ && !velocityGuess_.isNull()?Transform::getIdentity():Transform();
575  if(!(dt>0.0 || (dt == 0.0 && velocityGuess_.isNull())))
576  {
577  if(guessFromMotion_ && (!data.imageRaw().empty() || !data.laserScanRaw().isEmpty()))
578  {
579  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());
580  }
581  else if(_filteringStrategy==1)
582  {
583  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());
584  }
585  dt=0;
586  previousVelocities_.clear();
587  velocityGuess_.setNull();
588  }
589  if(!velocityGuess_.isNull())
590  {
591  if(guessFromMotion_)
592  {
593  if(_filteringStrategy == 1)
594  {
595  // use Kalman predict transform
596  float vx,vy,vz, vroll,vpitch,vyaw;
597  predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
598  guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
599  }
600  else
601  {
602  float vx,vy,vz, vroll,vpitch,vyaw;
603  velocityGuess_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
604  guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
605  }
606  }
607  else if(_filteringStrategy == 1)
608  {
609  predictKalmanFilter(dt);
610  }
611  }
612 
613  Transform imuCurrentTransform;
614  if(!guessIn.isNull())
615  {
616  guess = guessIn;
617  }
618  else if(!imus_.empty())
619  {
620  // replace orientation guess with IMU (if available)
621  imuCurrentTransform = Transform::getTransform(imus_, data.stamp());
622  if(!imuCurrentTransform.isNull() && !imuLastTransform_.isNull())
623  {
624  Transform orientation = imuLastTransform_.inverse() * imuCurrentTransform;
625  guess = Transform(
626  orientation.r11(), orientation.r12(), orientation.r13(), guess.x(),
627  orientation.r21(), orientation.r22(), orientation.r23(), guess.y(),
628  orientation.r31(), orientation.r32(), orientation.r33(), guess.z());
629  if(_force3DoF)
630  {
631  guess = guess.to3DoF();
632  }
633  }
634  else if(!imuLastTransform_.isNull())
635  {
636  UWARN("Could not find imu transform at %f", data.stamp());
637  }
638  }
639 
640  UTimer time;
641 
642  // Deskewing lidar
643  if( _deskewing &&
644  !data.laserScanRaw().empty() &&
645  data.laserScanRaw().hasTime() &&
646  dt > 0 &&
647  !guess.isNull())
648  {
649  UDEBUG("Deskewing begin");
650  // Recompute velocity
651  float vx,vy,vz, vroll,vpitch,vyaw;
652  guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
653 
654  // transform to velocity
655  vx /= dt;
656  vy /= dt;
657  vz /= dt;
658  vroll /= dt;
659  vpitch /= dt;
660  vyaw /= dt;
661 
662  if(!imus_.empty())
663  {
664  float scanTime =
665  data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
666  data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
667 
668  // replace orientation velocity based on IMU (if available)
669  Transform imuFirstScan = Transform::getTransform(imus_,
670  data.stamp() +
671  data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
672  Transform imuLastScan = Transform::getTransform(imus_,
673  data.stamp() +
674  data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
675  if(!imuFirstScan.isNull() && !imuLastScan.isNull())
676  {
677  Transform orientation = imuFirstScan.inverse() * imuLastScan;
678  orientation.getEulerAngles(vroll, vpitch, vyaw);
679  if(_force3DoF)
680  {
681  vroll=0;
682  vpitch=0;
683  vyaw /= scanTime;
684  }
685  else
686  {
687  vroll /= scanTime;
688  vpitch /= scanTime;
689  vyaw /= scanTime;
690  }
691  }
692  }
693 
694  Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
695  LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
696  if(!scanDeskewed.isEmpty())
697  {
698  data.setLaserScan(scanDeskewed);
699  }
700  info->timeDeskewing = time.ticks();
701  UDEBUG("Deskewing end");
702  }
703  if(data.laserScanRaw().isOrganized())
704  {
705  // Laser scans should be dense passing this point
706  data.setLaserScan(data.laserScanRaw().densify());
707  }
708 
709 
710  Transform t;
711  if(_imageDecimation > 1 && !data.imageRaw().empty())
712  {
713  // Decimation of images with calibrations
714  SensorData decimatedData = data;
715  int decimationDepth = _imageDecimation;
716  if( !data.cameraModels().empty() &&
717  data.cameraModels()[0].imageHeight()>0 &&
718  data.cameraModels()[0].imageWidth()>0)
719  {
720  // decimate from RGB image size
721  int targetSize = data.cameraModels()[0].imageHeight() / _imageDecimation;
722  if(targetSize >= data.depthRaw().rows)
723  {
724  decimationDepth = 1;
725  }
726  else
727  {
728  decimationDepth = (int)ceil(float(data.depthRaw().rows) / float(targetSize));
729  }
730  }
731  UDEBUG("decimation rgbOrLeft(rows=%d)=%d, depthOrRight(rows=%d)=%d", data.imageRaw().rows, _imageDecimation, data.depthOrRightRaw().rows, decimationDepth);
732 
733  cv::Mat rgbLeft = util2d::decimate(decimatedData.imageRaw(), _imageDecimation);
734  cv::Mat depthRight = util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth);
735  std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
736  for(unsigned int i=0; i<cameraModels.size(); ++i)
737  {
738  cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
739  }
740  if(!cameraModels.empty())
741  {
742  decimatedData.setRGBDImage(rgbLeft, depthRight, cameraModels);
743  }
744  else
745  {
746  std::vector<StereoCameraModel> stereoModels = decimatedData.stereoCameraModels();
747  for(unsigned int i=0; i<stereoModels.size(); ++i)
748  {
749  stereoModels[i].scale(1.0/double(_imageDecimation));
750  }
751  if(!stereoModels.empty())
752  {
753  decimatedData.setStereoImage(rgbLeft, depthRight, stereoModels);
754  }
755  }
756 
757 
758  // compute transform
759  t = this->computeTransform(decimatedData, guess, info);
760 
761  // transform back the keypoints in the original image
762  std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
763  double log2value = log(double(_imageDecimation))/log(2.0);
764  for(unsigned int i=0; i<kpts.size(); ++i)
765  {
766  kpts[i].pt.x *= _imageDecimation;
767  kpts[i].pt.y *= _imageDecimation;
768  kpts[i].size *= _imageDecimation;
769  kpts[i].octave += log2value;
770  }
771  data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
772  data.setLaserScan(decimatedData.laserScanRaw());
773 
774  if(info)
775  {
776  UASSERT(info->newCorners.size() == info->refCorners.size() || info->refCorners.empty());
777  for(unsigned int i=0; i<info->newCorners.size(); ++i)
778  {
779  info->newCorners[i].x *= _imageDecimation;
780  info->newCorners[i].y *= _imageDecimation;
781  if(!info->refCorners.empty())
782  {
783  info->refCorners[i].x *= _imageDecimation;
784  info->refCorners[i].y *= _imageDecimation;
785  }
786  }
787  for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
788  {
789  iter->second.pt.x *= _imageDecimation;
790  iter->second.pt.y *= _imageDecimation;
791  iter->second.size *= _imageDecimation;
792  iter->second.octave += log2value;
793  }
794  }
795  }
796  else if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty() || (this->canProcessAsyncIMU() && !data.imu().empty()))
797  {
798  t = this->computeTransform(data, guess, info);
799  }
800 
801  if(data.imageRaw().empty() && data.laserScanRaw().isEmpty() && !data.imu().empty())
802  {
803  return Transform(); // Return null on IMU-only updates
804  }
805 
806  if(info)
807  {
808  info->timeEstimation = time.ticks();
809  info->lost = t.isNull();
810  info->stamp = data.stamp();
811  info->interval = dt;
812  info->transform = t;
813  info->guess = guess;
814  if(_publishRAMUsage)
815  {
816  info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
817  }
818 
819  if(!data.groundTruth().isNull())
820  {
821  if(!previousGroundTruthPose_.isNull())
822  {
823  info->transformGroundTruth = previousGroundTruthPose_.inverse() * data.groundTruth();
824  }
825  previousGroundTruthPose_ = data.groundTruth();
826  }
827  }
828 
829  if(!t.isNull())
830  {
831  _resetCurrentCount = _resetCountdown;
832 
833  float vx,vy,vz, vroll,vpitch,vyaw;
834  t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
835 
836  // transform to velocity
837  if(dt)
838  {
839  vx /= dt;
840  vy /= dt;
841  vz /= dt;
842  vroll /= dt;
843  vpitch /= dt;
844  vyaw /= dt;
845  }
846 
847  if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
848  {
849  if(_filteringStrategy == 1)
850  {
851  if(velocityGuess_.isNull())
852  {
853  // reset Kalman
854  if(dt)
855  {
856  initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
857  }
858  else
859  {
860  initKalmanFilter(t);
861  }
862  }
863  else
864  {
865  // Kalman filtering
866  updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
867  }
868  }
869  else
870  {
871  if(particleFilters_.size())
872  {
873  // Particle filtering
874  UASSERT(particleFilters_.size()==6);
875  if(velocityGuess_.isNull())
876  {
877  particleFilters_[0]->init(vx);
878  particleFilters_[1]->init(vy);
879  particleFilters_[2]->init(vz);
880  particleFilters_[3]->init(vroll);
881  particleFilters_[4]->init(vpitch);
882  particleFilters_[5]->init(vyaw);
883  }
884  else
885  {
886  vx = particleFilters_[0]->filter(vx);
887  vy = particleFilters_[1]->filter(vy);
888  vyaw = particleFilters_[5]->filter(vyaw);
889 
890  if(!_holonomic)
891  {
892  // arc trajectory around ICR
893  float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
894  if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
895  {
896  vy = tmpY;
897  }
898  else
899  {
900  vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
901  }
902  }
903 
904  if(!_force3DoF)
905  {
906  vz = particleFilters_[2]->filter(vz);
907  vroll = particleFilters_[3]->filter(vroll);
908  vpitch = particleFilters_[4]->filter(vpitch);
909  }
910  }
911 
912  if(info)
913  {
914  info->timeParticleFiltering = time.ticks();
915  }
916  }
917  else if(!_holonomic)
918  {
919  // arc trajectory around ICR
920  vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
921  }
922 
923  if(_force3DoF)
924  {
925  vz = 0.0f;
926  vroll = 0.0f;
927  vpitch = 0.0f;
928  }
929  }
930 
931  if(dt)
932  {
933  t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
934  }
935  else
936  {
937  t = Transform(vx, vy, vz, vroll, vpitch, vyaw);
938  }
939 
940  if(info)
941  {
942  info->transformFiltered = t;
943  }
944  }
945 
946  if(data.stamp() == 0 && framesProcessed_ != 0)
947  {
948  UWARN("Null stamp detected");
949  }
950 
951  previousStamp_ = data.stamp();
952 
953  if(dt)
954  {
955  if(dt >= (guessSmoothingDelay_/2.0) || particleFilters_.size() || _filteringStrategy==1)
956  {
957  velocityGuess_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
958  previousVelocities_.clear();
959  }
960  else
961  {
962  // smooth velocity estimation over the past X seconds
963  std::vector<float> v(6);
964  v[0] = vx;
965  v[1] = vy;
966  v[2] = vz;
967  v[3] = vroll;
968  v[4] = vpitch;
969  v[5] = vyaw;
970  previousVelocities_.push_back(std::make_pair(v, data.stamp()));
971  while(previousVelocities_.size() > 1 && previousVelocities_.front().second < previousVelocities_.back().second-guessSmoothingDelay_)
972  {
973  previousVelocities_.pop_front();
974  }
975  velocityGuess_ = getMeanVelocity(previousVelocities_);
976  }
977  }
978  else
979  {
980  previousVelocities_.clear();
981  velocityGuess_.setNull();
982  }
983 
984  if(info)
985  {
986  distanceTravelled_ += t.getNorm();
987  info->distanceTravelled = distanceTravelled_;
988  info->guessVelocity = velocityGuess_;
989  }
990  ++framesProcessed_;
991 
992  imuLastTransform_ = imuCurrentTransform;
993 
994  return _pose *= t; // update
995  }
996  else if(_resetCurrentCount > 0)
997  {
998  UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
999 
1000  --_resetCurrentCount;
1001  if(_resetCurrentCount == 0)
1002  {
1003  UWARN("Odometry automatically reset to latest pose!");
1004  this->reset(_pose);
1005  _resetCurrentCount = _resetCountdown;
1006  if(info)
1007  {
1008  *info = OdometryInfo();
1009  }
1010  return this->computeTransform(data, Transform(), info);
1011  }
1012  }
1013 
1014  previousVelocities_.clear();
1015  velocityGuess_.setNull();
1016  previousStamp_ = 0;
1017 
1018  return Transform();
1019 }
1020 
1021 void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
1022 {
1023  UDEBUG("");
1024  // See OpenCV tutorial: http://docs.opencv.org/master/dc/d2c/tutorial_real_time_pose.html
1025  // See Kalman filter pose/orientation estimation theory: http://campar.in.tum.de/Chair/KalmanFilter
1026 
1027  // initialize the Kalman filter
1028  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'')
1029  int nMeasurements = 6; // the number of measured states (x',y',z',roll',pitch',yaw')
1030  if(_force3DoF)
1031  {
1032  nStates = 9; // the number of states (x,y,x',y',x'',y'',yaw,yaw',yaw'')
1033  nMeasurements = 3; // the number of measured states (x',y',yaw')
1034  }
1035  int nInputs = 0; // the number of action control
1036 
1037  /* From viso2, measurement covariance
1038  * static const boost::array<double, 36> STANDARD_POSE_COVARIANCE =
1039  { { 0.1, 0, 0, 0, 0, 0,
1040  0, 0.1, 0, 0, 0, 0,
1041  0, 0, 0.1, 0, 0, 0,
1042  0, 0, 0, 0.17, 0, 0,
1043  0, 0, 0, 0, 0.17, 0,
1044  0, 0, 0, 0, 0, 0.17 } };
1045  static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
1046  { { 0.05, 0, 0, 0, 0, 0,
1047  0, 0.05, 0, 0, 0, 0,
1048  0, 0, 0.05, 0, 0, 0,
1049  0, 0, 0, 0.09, 0, 0,
1050  0, 0, 0, 0, 0.09, 0,
1051  0, 0, 0, 0, 0, 0.09 } };
1052  */
1053 
1054 
1055  kalmanFilter_.init(nStates, nMeasurements, nInputs); // init Kalman Filter
1056  cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise)); // set process noise
1057  cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise)); // set measurement noise
1058  cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1)); // error covariance
1059 
1060  float x,y,z,roll,pitch,yaw;
1061  initialPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
1062 
1063  if(_force3DoF)
1064  {
1065  /* MEASUREMENT MODEL (velocity) */
1066  // [0 0 1 0 0 0 0 0 0]
1067  // [0 0 0 1 0 0 0 0 0]
1068  // [0 0 0 0 0 0 0 1 0]
1069  kalmanFilter_.measurementMatrix.at<float>(0,2) = 1; // x'
1070  kalmanFilter_.measurementMatrix.at<float>(1,3) = 1; // y'
1071  kalmanFilter_.measurementMatrix.at<float>(2,7) = 1; // yaw'
1072 
1073  kalmanFilter_.statePost.at<float>(0) = x;
1074  kalmanFilter_.statePost.at<float>(1) = y;
1075  kalmanFilter_.statePost.at<float>(6) = yaw;
1076 
1077  kalmanFilter_.statePost.at<float>(2) = vx;
1078  kalmanFilter_.statePost.at<float>(3) = vy;
1079  kalmanFilter_.statePost.at<float>(7) = vyaw;
1080  }
1081  else
1082  {
1083  /* MEASUREMENT MODEL (velocity) */
1084  // [0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0]
1085  // [0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0]
1086  // [0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]
1087  // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0]
1088  // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0]
1089  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0]
1090  kalmanFilter_.measurementMatrix.at<float>(0,3) = 1; // x'
1091  kalmanFilter_.measurementMatrix.at<float>(1,4) = 1; // y'
1092  kalmanFilter_.measurementMatrix.at<float>(2,5) = 1; // z'
1093  kalmanFilter_.measurementMatrix.at<float>(3,12) = 1; // roll'
1094  kalmanFilter_.measurementMatrix.at<float>(4,13) = 1; // pitch'
1095  kalmanFilter_.measurementMatrix.at<float>(5,14) = 1; // yaw'
1096 
1097  kalmanFilter_.statePost.at<float>(0) = x;
1098  kalmanFilter_.statePost.at<float>(1) = y;
1099  kalmanFilter_.statePost.at<float>(2) = z;
1100  kalmanFilter_.statePost.at<float>(9) = roll;
1101  kalmanFilter_.statePost.at<float>(10) = pitch;
1102  kalmanFilter_.statePost.at<float>(11) = yaw;
1103 
1104  kalmanFilter_.statePost.at<float>(3) = vx;
1105  kalmanFilter_.statePost.at<float>(4) = vy;
1106  kalmanFilter_.statePost.at<float>(5) = vz;
1107  kalmanFilter_.statePost.at<float>(12) = vroll;
1108  kalmanFilter_.statePost.at<float>(13) = vpitch;
1109  kalmanFilter_.statePost.at<float>(14) = vyaw;
1110  }
1111 }
1112 
1113 void Odometry::predictKalmanFilter(float dt, float * vx, float * vy, float * vz, float * vroll, float * vpitch, float * vyaw)
1114 {
1115  // Set transition matrix with current dt
1116  if(_force3DoF)
1117  {
1118  // 2D:
1119  // [1 0 dt 0 dt2 0 0 0 0] x
1120  // [0 1 0 dt 0 dt2 0 0 0] y
1121  // [0 0 1 0 dt 0 0 0 0] x'
1122  // [0 0 0 1 0 dt 0 0 0] y'
1123  // [0 0 0 0 1 0 0 0 0] x''
1124  // [0 0 0 0 0 0 0 0 0] y''
1125  // [0 0 0 0 0 0 1 dt dt2] yaw
1126  // [0 0 0 0 0 0 0 1 dt] yaw'
1127  // [0 0 0 0 0 0 0 0 1] yaw''
1128  kalmanFilter_.transitionMatrix.at<float>(0,2) = dt;
1129  kalmanFilter_.transitionMatrix.at<float>(1,3) = dt;
1130  kalmanFilter_.transitionMatrix.at<float>(2,4) = dt;
1131  kalmanFilter_.transitionMatrix.at<float>(3,5) = dt;
1132  kalmanFilter_.transitionMatrix.at<float>(0,4) = 0.5*pow(dt,2);
1133  kalmanFilter_.transitionMatrix.at<float>(1,5) = 0.5*pow(dt,2);
1134  // orientation
1135  kalmanFilter_.transitionMatrix.at<float>(6,7) = dt;
1136  kalmanFilter_.transitionMatrix.at<float>(7,8) = dt;
1137  kalmanFilter_.transitionMatrix.at<float>(6,8) = 0.5*pow(dt,2);
1138  }
1139  else
1140  {
1141  // [1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0 0] x
1142  // [0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0 0] y
1143  // [0 0 1 0 0 dt 0 0 dt2 0 0 0 0 0 0 0 0 0] z
1144  // [0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0 0] x'
1145  // [0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0 0] y'
1146  // [0 0 0 0 0 1 0 0 dt 0 0 0 0 0 0 0 0 0] z'
1147  // [0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0] x''
1148  // [0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0] y''
1149  // [0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0] z''
1150  // [0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0 0]
1151  // [0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2 0]
1152  // [0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0 dt2]
1153  // [0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0 0]
1154  // [0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt 0]
1155  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 dt]
1156  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0]
1157  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0]
1158  // [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1]
1159  // position
1160  kalmanFilter_.transitionMatrix.at<float>(0,3) = dt;
1161  kalmanFilter_.transitionMatrix.at<float>(1,4) = dt;
1162  kalmanFilter_.transitionMatrix.at<float>(2,5) = dt;
1163  kalmanFilter_.transitionMatrix.at<float>(3,6) = dt;
1164  kalmanFilter_.transitionMatrix.at<float>(4,7) = dt;
1165  kalmanFilter_.transitionMatrix.at<float>(5,8) = dt;
1166  kalmanFilter_.transitionMatrix.at<float>(0,6) = 0.5*pow(dt,2);
1167  kalmanFilter_.transitionMatrix.at<float>(1,7) = 0.5*pow(dt,2);
1168  kalmanFilter_.transitionMatrix.at<float>(2,8) = 0.5*pow(dt,2);
1169  // orientation
1170  kalmanFilter_.transitionMatrix.at<float>(9,12) = dt;
1171  kalmanFilter_.transitionMatrix.at<float>(10,13) = dt;
1172  kalmanFilter_.transitionMatrix.at<float>(11,14) = dt;
1173  kalmanFilter_.transitionMatrix.at<float>(12,15) = dt;
1174  kalmanFilter_.transitionMatrix.at<float>(13,16) = dt;
1175  kalmanFilter_.transitionMatrix.at<float>(14,17) = dt;
1176  kalmanFilter_.transitionMatrix.at<float>(9,15) = 0.5*pow(dt,2);
1177  kalmanFilter_.transitionMatrix.at<float>(10,16) = 0.5*pow(dt,2);
1178  kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
1179  }
1180 
1181  // First predict, to update the internal statePre variable
1182  UDEBUG("Predict");
1183  const cv::Mat & prediction = kalmanFilter_.predict();
1184 
1185  if(vx)
1186  *vx = prediction.at<float>(3); // x'
1187  if(vy)
1188  *vy = prediction.at<float>(4); // y'
1189  if(vz)
1190  *vz = _force3DoF?0.0f:prediction.at<float>(5); // z'
1191  if(vroll)
1192  *vroll = _force3DoF?0.0f:prediction.at<float>(12); // roll'
1193  if(vpitch)
1194  *vpitch = _force3DoF?0.0f:prediction.at<float>(13); // pitch'
1195  if(vyaw)
1196  *vyaw = prediction.at<float>(_force3DoF?7:14); // yaw'
1197 }
1198 
1199 void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
1200 {
1201  // Set measurement to predict
1202  cv::Mat measurements;
1203  if(!_force3DoF)
1204  {
1205  measurements = cv::Mat(6,1,CV_32FC1);
1206  measurements.at<float>(0) = vx; // x'
1207  measurements.at<float>(1) = vy; // y'
1208  measurements.at<float>(2) = vz; // z'
1209  measurements.at<float>(3) = vroll; // roll'
1210  measurements.at<float>(4) = vpitch; // pitch'
1211  measurements.at<float>(5) = vyaw; // yaw'
1212  }
1213  else
1214  {
1215  measurements = cv::Mat(3,1,CV_32FC1);
1216  measurements.at<float>(0) = vx; // x'
1217  measurements.at<float>(1) = vy; // y'
1218  measurements.at<float>(2) = vyaw; // yaw',
1219  }
1220 
1221  // The "correct" phase that is going to use the predicted value and our measurement
1222  UDEBUG("Correct");
1223  const cv::Mat & estimated = kalmanFilter_.correct(measurements);
1224 
1225 
1226  vx = estimated.at<float>(3); // x'
1227  vy = estimated.at<float>(4); // y'
1228  vz = _force3DoF?0.0f:estimated.at<float>(5); // z'
1229  vroll = _force3DoF?0.0f:estimated.at<float>(12); // roll'
1230  vpitch = _force3DoF?0.0f:estimated.at<float>(13); // pitch'
1231  vyaw = estimated.at<float>(_force3DoF?7:14); // yaw'
1232 }
1233 
1234 } /* 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:1199
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:613
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:1021
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:2358
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:1113
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:500
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:3553
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
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:1040
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:40
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 Jul 1 2024 02:42:31