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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:29