OdometryVINS.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/OdometryVINS.h"
29 #include "rtabmap/core/OdometryInfo.h"
30 #include "rtabmap/core/util3d_transforms.h"
31 #include "rtabmap/utilite/ULogger.h"
32 #include "rtabmap/utilite/UTimer.h"
33 #include "rtabmap/utilite/UStl.h"
34 #include "rtabmap/utilite/UThread.h"
35 #include "rtabmap/utilite/UDirectory.h"
36 #include <opencv2/imgproc/types_c.h>
37 
38 #ifdef RTABMAP_VINS
39 #include <estimator/estimator.h>
40 #include <estimator/parameters.h>
41 #include <camodocal/camera_models/PinholeCamera.h>
42 #include <camodocal/camera_models/EquidistantCamera.h>
43 #include <utility/visualization.h>
44 #endif
45 
46 namespace rtabmap {
47 
48 #ifdef RTABMAP_VINS
49 class VinsEstimator: public Estimator
50 {
51 public:
52  VinsEstimator(
53  const Transform & imuLocalTransform,
54  const StereoCameraModel & model,
55  bool rectified) : Estimator()
56  {
57  MULTIPLE_THREAD = 0;
58  setParameter();
59 
60  //overwrite camera calibration only if received model is radtan, otherwise use config
61  UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
62 
63  if( (NUM_OF_CAM == 2 && model.left().D_raw().cols == 4 && model.right().D_raw().cols == 4) ||
64  (NUM_OF_CAM == 1 && model.left().D_raw().cols == 4))
65  {
66  UWARN("Overwriting VINS camera calibration config with received pinhole model... rectified=%d", rectified?1:0);
67  featureTracker.m_camera.clear();
68 
69  camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
70  camodocal::PinholeCamera::Parameters params(
71  model.name(),
72  model.left().imageWidth(), model.left().imageHeight(),
73  rectified?0:model.left().D_raw().at<double>(0,0),
74  rectified?0:model.left().D_raw().at<double>(0,1),
75  rectified?0:model.left().D_raw().at<double>(0,2),
76  rectified?0:model.left().D_raw().at<double>(0,3),
77  rectified?model.left().fx():model.left().K_raw().at<double>(0,0),
78  rectified?model.left().fy():model.left().K_raw().at<double>(1,1),
79  rectified?model.left().cx():model.left().K_raw().at<double>(0,2),
80  rectified?model.left().cy():model.left().K_raw().at<double>(1,2));
81  camera->setParameters(params);
82  featureTracker.m_camera.push_back(camera);
83 
84  if(NUM_OF_CAM == 2)
85  {
86  camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
87  camodocal::PinholeCamera::Parameters params(
88  model.name(),
89  model.right().imageWidth(), model.right().imageHeight(),
90  rectified?0:model.right().D_raw().at<double>(0,0),
91  rectified?0:model.right().D_raw().at<double>(0,1),
92  rectified?0:model.right().D_raw().at<double>(0,2),
93  rectified?0:model.right().D_raw().at<double>(0,3),
94  rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
95  rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
96  rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
97  rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
98  camera->setParameters(params);
99  featureTracker.m_camera.push_back(camera);
100  }
101  }
102  else if(rectified)
103  {
104  UWARN("Images are rectified but received calibration cannot be "
105  "used, make sure calibration in config file doesn't have "
106  "distortion or send raw images to VINS odometry.");
107  if(!featureTracker.m_camera.empty())
108  {
109  if(featureTracker.m_camera.front()->imageWidth() != model.left().imageWidth() ||
110  featureTracker.m_camera.front()->imageHeight() != model.left().imageHeight())
111  {
112  UERROR("Received images don't have same size (%dx%d) than in the config file (%dx%d)!",
113  model.left().imageWidth(),
114  model.left().imageHeight(),
115  featureTracker.m_camera.front()->imageWidth(),
116  featureTracker.m_camera.front()->imageHeight());
117  }
118  }
119  }
120 
121  Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
122 
123  tic[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
124  ric[0] = imuCam0.toEigen4d().block<3,3>(0,0);
125 
126  if(NUM_OF_CAM == 2)
127  {
128  Transform cam0cam1;
129  if(rectified)
130  {
131  cam0cam1 = Transform(
132  1, 0, 0, model.baseline(),
133  0, 1, 0, 0,
134  0, 0, 1, 0);
135  }
136  else
137  {
138  cam0cam1 = model.stereoTransform().inverse();
139  }
140  UASSERT(!cam0cam1.isNull());
141  Transform imuCam1 = imuCam0 * cam0cam1;
142 
143  tic[1] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
144  ric[1] = imuCam1.toEigen4d().block<3,3>(0,0);
145  }
146 
147  for (int i = 0; i < NUM_OF_CAM; i++)
148  {
149  cout << " exitrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl;
150  }
151  f_manager.setRic(ric);
152  ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
153  ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
154  ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
155  td = TD;
156  g = G;
157  cout << "set g " << g.transpose() << endl;
158  }
159 
160  // Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
161  void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
162  {
163  inputImageCnt++;
164  map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
165  TicToc featureTrackerTime;
166  if(_img1.empty())
167  featureFrame = featureTracker.trackImage(t, _img);
168  else
169  featureFrame = featureTracker.trackImage(t, _img, _img1);
170  //printf("featureTracker time: %f\n", featureTrackerTime.toc());
171 
172  //if(MULTIPLE_THREAD)
173  //{
174  // if(inputImageCnt % 2 == 0)
175  // {
176  // mBuf.lock();
177  // featureBuf.push(make_pair(t, featureFrame));
178  // mBuf.unlock();
179  // }
180  //}
181  //else
182  {
183  mBuf.lock();
184  featureBuf.push(make_pair(t, featureFrame));
185  mBuf.unlock();
186  TicToc processTime;
187  processMeasurements();
188  UDEBUG("VINS process time: %f", processTime.toc());
189  }
190 
191  }
192 
193  // Copy of original inputIMU() but with publisher commented
194  void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity)
195  {
196  mBuf.lock();
197  accBuf.push(make_pair(t, linearAcceleration));
198  gyrBuf.push(make_pair(t, angularVelocity));
199  //printf("input imu with time %f \n", t);
200  mBuf.unlock();
201 
202  fastPredictIMU(t, linearAcceleration, angularVelocity);
203  //if (solver_flag == NON_LINEAR)
204  // pubLatestOdometry(latest_P, latest_Q, latest_V, t);
205  }
206 
207  // Copy of original processMeasurements() but with publishers commented and threading disabled
208  void processMeasurements()
209  {
210  //while (1)
211  {
212  //printf("process measurments\n");
213  pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
214  vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
215  if(!featureBuf.empty())
216  {
217  feature = featureBuf.front();
218  curTime = feature.first + td;
219  //while(1)
220  //{
221  if (!((!USE_IMU || IMUAvailable(feature.first + td))))
222  //if ((!USE_IMU || IMUAvailable(feature.first + td)))
223  // break;
224  //else
225  {
226  printf("wait for imu ... \n");
227  //if (! MULTIPLE_THREAD)
228  return;
229  //std::chrono::milliseconds dura(5);
230  //std::this_thread::sleep_for(dura);
231  }
232  //}
233  mBuf.lock();
234  if(USE_IMU)
235  getIMUInterval(prevTime, curTime, accVector, gyrVector);
236 
237  featureBuf.pop();
238  mBuf.unlock();
239 
240  if(USE_IMU)
241  {
242  if(!initFirstPoseFlag)
243  initFirstIMUPose(accVector);
244  UDEBUG("accVector.size() = %d", accVector.size());
245  for(size_t i = 0; i < accVector.size(); i++)
246  {
247  double dt;
248  if(i == 0)
249  dt = accVector[i].first - prevTime;
250  else if (i == accVector.size() - 1)
251  dt = curTime - accVector[i - 1].first;
252  else
253  dt = accVector[i].first - accVector[i - 1].first;
254  processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
255  }
256  }
257 
258  processImage(feature.second, feature.first);
259 
260  prevTime = curTime;
261 
262  printStatistics(*this, 0);
263 
264  //std_msgs::Header header;
265  //header.frame_id = "world";
266  //header.stamp = ros::Time(feature.first);
267 
268  //pubOdometry(*this, header);
269  //pubKeyPoses(*this, header);
270  //pubCameraPose(*this, header);
271  //pubPointCloud(*this, header);
272  //pubKeyframe(*this);
273  //pubTF(*this, header);
274  }
275 
276  //if (! MULTIPLE_THREAD)
277  // break;
278 
279  //std::chrono::milliseconds dura(2);
280  //std::this_thread::sleep_for(dura);
281  }
282  }
283 };
284 #endif
285 
286 OdometryVINS::OdometryVINS(const ParametersMap & parameters) :
287  Odometry(parameters)
288 #ifdef RTABMAP_VINS
289  ,
290  vinsEstimator_(0),
291  initGravity_(false),
292  previousPose_(Transform::getIdentity())
293 #endif
294 {
295 #ifdef RTABMAP_VINS
296  // intialize
297  std::string configFilename;
298  Parameters::parse(parameters, Parameters::kOdomVINSConfigPath(), configFilename);
299  if(configFilename.empty())
300  {
301  UERROR("VINS config file is empty (%s=%s)!",
302  Parameters::kOdomVINSConfigPath().c_str(),
303  Parameters::kOdomVINSConfigPath().c_str());
304  }
305  else
306  {
307  readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir()));
308  }
309 #endif
310 }
311 
312 OdometryVINS::~OdometryVINS()
313 {
314 #ifdef RTABMAP_VINS
315  delete vinsEstimator_;
316 #endif
317 }
318 
319 void OdometryVINS::reset(const Transform & initialPose)
320 {
321  Odometry::reset(initialPose);
322 #ifdef RTABMAP_VINS
323  if(!initGravity_)
324  {
325  delete vinsEstimator_;
326  vinsEstimator_ = 0;
327  previousPose_.setIdentity();
328  lastImu_ = IMU();
329  previousLocalTransform_.setNull();
330  }
331  initGravity_ = false;
332 #endif
333 }
334 
335 // return not null transform if odometry is correctly computed
336 Transform OdometryVINS::computeTransform(
337  SensorData & data,
338  const Transform & guess,
339  OdometryInfo * info)
340 {
341  Transform t;
342 #ifdef RTABMAP_VINS
343  UTimer timer;
344 
345  if(USE_IMU!=0 && !data.imu().empty())
346  {
347  double t = data.stamp();
348  double dx = data.imu().linearAcceleration().val[0];
349  double dy = data.imu().linearAcceleration().val[1];
350  double dz = data.imu().linearAcceleration().val[2];
351  double rx = data.imu().angularVelocity().val[0];
352  double ry = data.imu().angularVelocity().val[1];
353  double rz = data.imu().angularVelocity().val[2];
354  Vector3d acc(dx, dy, dz);
355  Vector3d gyr(rx, ry, rz);
356 
357  UDEBUG("IMU update stamp=%f", data.stamp());
358 
359  if(vinsEstimator_ != 0)
360  {
361  vinsEstimator_->inputIMU(t, acc, gyr);
362  }
363  else
364  {
365  lastImu_ = data.imu();
366  UWARN("Waiting an image for initialization...");
367  }
368  }
369 
370  if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
371  {
372  if(USE_IMU==1 && lastImu_.localTransform().isNull())
373  {
374  UWARN("Waiting IMU for initialization...");
375  return t;
376  }
377  if(vinsEstimator_ == 0)
378  {
379  // intialize
380  vinsEstimator_ = new VinsEstimator(
381  lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
382  data.stereoCameraModels()[0],
383  this->imagesAlreadyRectified());
384  }
385 
386  UDEBUG("Image update stamp=%f", data.stamp());
387  cv::Mat left;
388  cv::Mat right;
389  if(data.imageRaw().type() == CV_8UC3)
390  {
391  cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY);
392  }
393  else if(data.imageRaw().type() == CV_8UC1)
394  {
395  left = data.imageRaw().clone();
396  }
397  else
398  {
399  UFATAL("Not supported color type!");
400  }
401  if(data.rightRaw().type() == CV_8UC3)
402  {
403  cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
404  }
405  else if(data.rightRaw().type() == CV_8UC1)
406  {
407  right = data.rightRaw().clone();
408  }
409  else
410  {
411  UFATAL("Not supported color type!");
412  }
413 
414  vinsEstimator_->inputImage(data.stamp(), left, right);
415 
416  if(vinsEstimator_->solver_flag == Estimator::NON_LINEAR)
417  {
418  Quaterniond tmp_Q;
419  tmp_Q = Quaterniond(vinsEstimator_->Rs[WINDOW_SIZE]);
420  Transform p(
421  vinsEstimator_->Ps[WINDOW_SIZE].x(),
422  vinsEstimator_->Ps[WINDOW_SIZE].y(),
423  vinsEstimator_->Ps[WINDOW_SIZE].z(),
424  tmp_Q.x(),
425  tmp_Q.y(),
426  tmp_Q.z(),
427  tmp_Q.w());
428 
429  if(!p.isNull())
430  {
431  if(!lastImu_.localTransform().isNull())
432  {
433  p = p * lastImu_.localTransform().inverse();
434  }
435 
436  if(this->getPose().rotation().isIdentity())
437  {
438  initGravity_ = true;
439  this->reset(this->getPose()*p.rotation());
440  }
441 
442  if(previousPose_.isIdentity())
443  {
444  previousPose_ = p;
445  }
446 
447  // make it incremental
448  Transform previousPoseInv = previousPose_.inverse();
449  t = previousPoseInv*p;
450  previousPose_ = p;
451 
452  if(info)
453  {
454  info->type = this->getType();
455  info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
456  info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
457 
458  // feature map
459  Transform fixT = this->getPose()*previousPoseInv;
460  for (auto &it_per_id : vinsEstimator_->f_manager.feature)
461  {
462  int used_num;
463  used_num = it_per_id.feature_per_frame.size();
464  if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
465  continue;
466  if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)
467  continue;
468  int imu_i = it_per_id.start_frame;
469  Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth;
470  Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i];
471 
472  cv::Point3f p;
473  p.x = w_pts_i(0);
474  p.y = w_pts_i(1);
475  p.z = w_pts_i(2);
476  p = util3d::transformPoint(p, fixT);
477  info->localMap.insert(std::make_pair(it_per_id.feature_id, p));
478 
479  if(this->imagesAlreadyRectified())
480  {
481  cv::Point2f pt;
482  data.stereoCameraModels()[0].left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
483  info->reg.inliersIDs.push_back(info->newCorners.size());
484  info->newCorners.push_back(pt);
485  }
486  }
487  info->features = info->newCorners.size();
488  info->localMapSize = info->localMap.size();
489  }
490  UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
491  }
492  }
493  else
494  {
495  UWARN("VINS not yet initialized... waiting to get enough IMU messages");
496  }
497  }
498  else if(!data.imageRaw().empty() && !data.depthRaw().empty())
499  {
500  UERROR("VINS-Fusion doesn't work with RGB-D data, stereo images are required!");
501  }
502  else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
503  {
504  UERROR("VINS-Fusion requires stereo images!");
505  }
506  else if(data.imu().empty())
507  {
508  UERROR("VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
509  }
510 #else
511  UERROR("RTAB-Map is not built with VINS support! Select another visual odometry approach.");
512 #endif
513  return t;
514 }
515 
516 } // namespace rtabmap
UDirectory::homeDir
static std::string homeDir()
Definition: UDirectory.cpp:355
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
UTimer
Definition: UTimer.h:46
rtabmap::Odometry::imagesAlreadyRectified
bool imagesAlreadyRectified() const
Definition: Odometry.h:82
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:203
rtabmap::Odometry::framesProcessed
unsigned int framesProcessed() const
Definition: Odometry.h:81
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:126
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
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::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
rtabmap::OdometryVINS::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryVINS.cpp:336
rtabmap::OdometryVINS::getType
virtual Odometry::Type getType()
Definition: OdometryVINS.h:44
UFATAL
#define UFATAL(...)
rtabmap::OdometryVINS::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryVINS.cpp:319
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:211
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
util3d_transforms.h
params
params
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::IMU::angularVelocity
const cv::Vec3d & angularVelocity() const
Definition: IMU.h:56
false
#define false
Definition: ConvertUTF.c:56
UDEBUG
#define UDEBUG(...)
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::OdometryVINS::OdometryVINS
OdometryVINS(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryVINS.cpp:286
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:131
trace.model
model
Definition: trace.py:4
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::IMU::linearAcceleration
const cv::Vec3d linearAcceleration() const
Definition: IMU.h:59
rtabmap::Transform::size
int size() const
Definition: Transform.h:90
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
rtabmap::SensorData::imu
const IMU & imu() const
Definition: SensorData.h:290
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UINFO
#define UINFO(...)

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