OdometryOkvis.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/OdometryOkvis.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/UFile.h"
36 #include "rtabmap/utilite/UDirectory.h"
37 #include <opencv2/imgproc/types_c.h>
38 
39 #ifdef RTABMAP_OKVIS
40 #include <iostream>
41 #include <fstream>
42 #include <stdlib.h>
43 #include <memory>
44 #include <functional>
45 #include <atomic>
46 
47 #include <Eigen/Core>
48 #pragma GCC diagnostic push
49 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
50 #pragma GCC diagnostic ignored "-Woverloaded-virtual"
51 #include <opencv2/opencv.hpp>
52 #pragma GCC diagnostic pop
53 #include <okvis/VioParametersReader.hpp>
54 #include <okvis/ThreadedKFVio.hpp>
55 #include <okvis/cameras/PinholeCamera.hpp>
56 #include <okvis/cameras/NoDistortion.hpp>
57 #include <okvis/cameras/RadialTangentialDistortion.hpp>
58 #include <okvis/cameras/EquidistantDistortion.hpp>
59 #include <okvis/cameras/RadialTangentialDistortion8.hpp>
60 #include <boost/filesystem.hpp>
61 #endif
62 
63 namespace rtabmap {
64 
65 #ifdef RTABMAP_OKVIS
66 class OkvisCallbackHandler
67 {
68 public:
69  OkvisCallbackHandler()
70  {
71  }
72 
73  Transform getLastTransform()
74  {
75  UDEBUG("");
76  Transform tf;
77  mutex_.lock();
78  tf = transform_;
79  mutex_.unlock();
80 
81  return tf;
82  }
83 
84  std::map<int, cv::Point3f> getLastLandmarks()
85  {
86  UDEBUG("");
87  std::map<int, cv::Point3f> landmarks;
88  mutexLandmarks_.lock();
89  landmarks = landmarks_;
90  mutexLandmarks_.unlock();
91  return landmarks;
92  }
93 
94 public:
95  void fullStateCallback(
96  const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
97  const Eigen::Matrix<double, 9, 1> & /*speedAndBiases*/,
98  const Eigen::Matrix<double, 3, 1> & /*omega_S*/)
99  {
100  UDEBUG("");
101  Transform tf = Transform::fromEigen4d(T_WS.T());
102 
103  mutex_.lock();
104  transform_ = tf;
105  mutex_.unlock();
106  }
107 
108  void landmarksCallback(const okvis::Time & t,
109  const okvis::MapPointVector & landmarksVector,
110  const okvis::MapPointVector & /*transferredLandmarks*/)
111  {
112  UDEBUG("");
113  mutexLandmarks_.lock();
114  landmarks_.clear();
115  for(unsigned int i=0; i<landmarksVector.size(); ++i)
116  {
117  landmarks_.insert(std::make_pair((int)landmarksVector[i].id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2])));
118  }
119  mutexLandmarks_.unlock();
120  }
121 
122 
123 
124 private:
125  Transform transform_;
126  std::map<int, cv::Point3f> landmarks_;
127  UMutex mutex_;
128  UMutex mutexLandmarks_;
129 };
130 #endif
131 
132 OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
133  Odometry(parameters),
134 #ifdef RTABMAP_OKVIS
135  okvisCallbackHandler_(new OkvisCallbackHandler),
136  okvisEstimator_(0),
137  imagesProcessed_(0),
138  initGravity_(false),
139 #endif
140  okvisParameters_(parameters),
141  previousPose_(Transform::getIdentity())
142 {
143 #ifdef RTABMAP_OKVIS
144  Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
145  configFilename_ = uReplaceChar(configFilename_, '~', UDirectory::homeDir());
146  if(configFilename_.empty() || !UFile::exists(configFilename_))
147  {
148  UERROR("OKVIS config file is empty or doesn't exist (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
149  }
150 #endif
151 }
152 
153 OdometryOkvis::~OdometryOkvis()
154 {
155  UDEBUG("");
156 #ifdef RTABMAP_OKVIS
157  delete okvisEstimator_;
158  delete okvisCallbackHandler_;
159 #endif
160 }
161 
162 void OdometryOkvis::reset(const Transform & initialPose)
163 {
164  Odometry::reset(initialPose);
165 #ifdef RTABMAP_OKVIS
166  if(!initGravity_)
167  {
168  if(okvisEstimator_)
169  {
170  delete okvisEstimator_;
171  okvisEstimator_ = 0;
172  }
173  lastImu_ = IMU();
174  imagesProcessed_ = 0;
175  previousPose_.setIdentity();
176 
177  delete okvisCallbackHandler_;
178  okvisCallbackHandler_ = new OkvisCallbackHandler();
179  }
180  initGravity_ = false;
181 #endif
182 }
183 
184 // return not null transform if odometry is correctly computed
185 Transform OdometryOkvis::computeTransform(
186  SensorData & data,
187  const Transform & guess,
188  OdometryInfo * info)
189 {
190  UDEBUG("");
191  Transform t;
192 #ifdef RTABMAP_OKVIS
193  UTimer timer;
194 
195  okvis::Time timeOkvis = okvis::Time(data.stamp());
196 
197  if(!data.imu().empty())
198  {
199  UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
200  data.imu().linearAcceleration()[0],
201  data.imu().linearAcceleration()[1],
202  data.imu().linearAcceleration()[2],
203  data.imu().angularVelocity()[0],
204  data.imu().angularVelocity()[1],
205  data.imu().angularVelocity()[2]);
206  if(okvisEstimator_ != 0)
207  {
208  Eigen::Vector3d acc(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
209  Eigen::Vector3d ang(data.imu().angularVelocity()[0], data.imu().angularVelocity()[1], data.imu().angularVelocity()[2]);
210  okvisEstimator_->addImuMeasurement(timeOkvis, acc, ang);
211  }
212  else
213  {
214  UWARN("Ignoring IMU, waiting for an image to initialize...");
215  lastImu_ = data.imu();
216  }
217  }
218 
219  if(!data.imageRaw().empty())
220  {
221  UDEBUG("Image update stamp=%f", data.stamp());
222  std::vector<cv::Mat> images;
223  std::vector<CameraModel> models;
224  if(data.stereoCameraModels().size() ==1 && data.stereoCameraModels()[0].isValidForProjection())
225  {
226  images.push_back(data.imageRaw());
227  images.push_back(data.rightRaw());
228  CameraModel mleft = data.stereoCameraModels()[0].left();
229  // should be transform between IMU and camera
230  mleft.setLocalTransform(lastImu_.localTransform().inverse()*mleft.localTransform());
231  models.push_back(mleft);
232  CameraModel mright = data.stereoCameraModels()[0].right();
233 
234  // To support not rectified images
235  if(!imagesAlreadyRectified())
236  {
237  cv::Mat R = data.stereoCameraModels()[0].R();
238  cv::Mat T = data.stereoCameraModels()[0].T();
239  UASSERT(R.cols==3 && R.rows == 3);
240  UASSERT(T.cols==1 && T.rows == 3);
241  Transform extrinsics(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0,0),
242  R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1,0),
243  R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2,0));
244  mright.setLocalTransform(mleft.localTransform() * extrinsics.inverse());
245  }
246  else
247  {
248  Transform extrinsics(1, 0, 0, 0,
249  0, 1, 0, data.stereoCameraModels()[0].baseline(),
250  0, 0, 1, 0);
251  mright.setLocalTransform(extrinsics * mleft.localTransform());
252  }
253 
254 
255  models.push_back(mright);
256  }
257  else
258  {
259  UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
260  int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
261  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
262  {
263  if(data.cameraModels()[i].isValidForProjection())
264  {
265  images.push_back(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
266  CameraModel m = data.cameraModels()[i];
267  // should be transform between IMU and camera
268  m.setLocalTransform(lastImu_.localTransform().inverse()*m.localTransform());
269  models.push_back(m);
270  }
271  }
272  }
273 
274  bool imageUpdated = false;
275  if(images.size())
276  {
277  // initialization
278  if(okvisEstimator_ == 0)
279  {
280  UDEBUG("Initialization");
281  if(lastImu_.empty())
282  {
283  UWARN("Ignoring Image, waiting for imu to initialize...");
284  return t;
285  }
286 
287  okvis::VioParameters parameters;
288  if(configFilename_.empty() || !UFile::exists(configFilename_))
289  {
290  UERROR("OKVIS config file is empty or doesn't exist (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
291  return t;
292  }
293  else
294  {
295  okvis::VioParametersReader vio_parameters_reader(uReplaceChar(configFilename_, '~', UDirectory::homeDir()));
296  vio_parameters_reader.getParameters(parameters);
297  if(parameters.nCameraSystem.numCameras() > 0)
298  {
299  UWARN("Camera calibration included in OKVIS is ignored as calibration from received images will be used instead.");
300  }
301  parameters.nCameraSystem = okvis::cameras::NCameraSystem();
302  }
303 
304  parameters.publishing.publishRate = parameters.imu.rate; // rate at which odometry updates are published only works properly if imu_rate/publish_rate is an integer!!
305  parameters.publishing.publishLandmarks = true; // select, if you want to publish landmarks at all
306  parameters.publishing.publishImuPropagatedState = true; // Should the state that is propagated with IMU messages be published? Or just the optimized ones?
307  parameters.publishing.landmarkQualityThreshold = 1.0e-2; // landmark with lower quality will not be published
308  parameters.publishing.maxLandmarkQuality = 0.05; // landmark with higher quality will be published with the maximum colour intensity
309  parameters.publishing.trackedBodyFrame = okvis::FrameName::B; // B or S, the frame of reference that will be expressed relative to the selected worldFrame
310  parameters.publishing.velocitiesFrame = okvis::FrameName::B; // Wc, B or S, the frames in which the velocities of the selected trackedBodyFrame will be expressed in
311 
312  // non-hard coded parameters
313  parameters.imu.T_BS = okvis::kinematics::Transformation(lastImu_.localTransform().toEigen4d());
314  UINFO("Images are already rectified = %s", imagesAlreadyRectified()?"true":"false");
315  for(unsigned int i=0; i<models.size(); ++i)
316  {
317  okvis::cameras::NCameraSystem::DistortionType distType = okvis::cameras::NCameraSystem::NoDistortion;
318  std::shared_ptr<const okvis::cameras::CameraBase> cam;
319  if(!imagesAlreadyRectified())
320  {
321  if(models[i].D_raw().cols == 8)
322  {
323  okvis::cameras::RadialTangentialDistortion8 dist(
324  models[i].D_raw().at<double>(0,0),
325  models[i].D_raw().at<double>(0,1),
326  models[i].D_raw().at<double>(0,2),
327  models[i].D_raw().at<double>(0,3),
328  models[i].D_raw().at<double>(0,4),
329  models[i].D_raw().at<double>(0,5),
330  models[i].D_raw().at<double>(0,6),
331  models[i].D_raw().at<double>(0,7));
332  cam.reset(
333  new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>(
334  models[i].imageWidth(),
335  models[i].imageHeight(),
336  models[i].K_raw().at<double>(0,0),
337  models[i].K_raw().at<double>(1,1),
338  models[i].K_raw().at<double>(0,2),
339  models[i].K_raw().at<double>(1,2),
340  dist));
341  distType = okvis::cameras::NCameraSystem::RadialTangential8;
342  UINFO("RadialTangential8");
343  }
344  else if(models[i].D_raw().cols == 6)
345  {
346  okvis::cameras::EquidistantDistortion dist(
347  models[i].D_raw().at<double>(0,0),
348  models[i].D_raw().at<double>(0,1),
349  models[i].D_raw().at<double>(0,4),
350  models[i].D_raw().at<double>(0,5));
351  cam.reset(new okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>(
352  models[i].imageWidth(),
353  models[i].imageHeight(),
354  models[i].K_raw().at<double>(0,0),
355  models[i].K_raw().at<double>(1,1),
356  models[i].K_raw().at<double>(0,2),
357  models[i].K_raw().at<double>(1,2),
358  dist));
359  distType = okvis::cameras::NCameraSystem::Equidistant;
360  UINFO("Equidistant");
361  }
362  else if(models[i].D_raw().cols >= 4)
363  {
364  // To support not rectified images
365  okvis::cameras::RadialTangentialDistortion dist(
366  models[i].D_raw().at<double>(0,0),
367  models[i].D_raw().at<double>(0,1),
368  models[i].D_raw().at<double>(0,2),
369  models[i].D_raw().at<double>(0,3));
370  cam.reset(
371  new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
372  models[i].imageWidth(),
373  models[i].imageHeight(),
374  models[i].K_raw().at<double>(0,0),
375  models[i].K_raw().at<double>(1,1),
376  models[i].K_raw().at<double>(0,2),
377  models[i].K_raw().at<double>(1,2),
378  dist));
379  distType = okvis::cameras::NCameraSystem::RadialTangential;
380  UINFO("RadialTangential");
381  }
382  }
383  else // no distortion, rectified images
384  {
385  okvis::cameras::RadialTangentialDistortion dist(0,0,0,0);
386  cam.reset(
387  new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
388  models[i].imageWidth(),
389  models[i].imageHeight(),
390  models[i].K().at<double>(0,0),
391  models[i].K().at<double>(1,1),
392  models[i].K().at<double>(0,2),
393  models[i].K().at<double>(1,2),
394  dist));
395  distType = okvis::cameras::NCameraSystem::RadialTangential;
396  }
397 
398  if(cam.get())
399  {
400  UINFO("model %d: %s", i, models[i].localTransform().prettyPrint().c_str());
401 
402  Eigen::Vector3d r(models[i].localTransform().x(), models[i].localTransform().y(), models[i].localTransform().z());
403  parameters.nCameraSystem.addCamera(
404  std::shared_ptr<const okvis::kinematics::Transformation>(new okvis::kinematics::Transformation(r, models[i].localTransform().getQuaterniond().normalized())),
405  cam,
406  distType);
407  }
408  }
409 
410  okvisEstimator_ = new okvis::ThreadedKFVio(parameters);
411 
412  okvisEstimator_->setFullStateCallback(
413  std::bind(&OkvisCallbackHandler::fullStateCallback, okvisCallbackHandler_,
414  std::placeholders::_1, std::placeholders::_2,
415  std::placeholders::_3, std::placeholders::_4));
416 
417  okvisEstimator_->setLandmarksCallback(
418  std::bind(&OkvisCallbackHandler::landmarksCallback, okvisCallbackHandler_,
419  std::placeholders::_1, std::placeholders::_2,
420  std::placeholders::_3));
421 
422  okvisEstimator_->setBlocking(true);
423  }
424 
425  for(unsigned int i=0; i<images.size(); ++i)
426  {
427  cv::Mat gray;
428  if(images[i].type() == CV_8UC3)
429  {
430  cv::cvtColor(images[i], gray, CV_BGR2GRAY);
431  }
432  else if(images[i].type() == CV_8UC1)
433  {
434  gray = images[i];
435  }
436  else
437  {
438  UFATAL("Not supported color type!");
439  }
440 
441  imageUpdated = okvisEstimator_->addImage(timeOkvis, i, gray);
442  if(!imageUpdated)
443  {
444  UWARN("Image update with stamp %f delayed...", data.stamp());
445  }
446  }
447  if(imageUpdated)
448  {
449  ++imagesProcessed_;
450  }
451  }
452 
453  if(imageUpdated && imagesProcessed_ > 10)
454  {
455  Transform fixPos(-1,0,0,0, 0,-1,0,0, 0,0,1,0);
456  Transform fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0);
457  Transform p = okvisCallbackHandler_->getLastTransform();
458  if(!p.isNull())
459  {
460  p = fixPos * p * fixRot;
461 
462  if(this->getPose().rotation().isIdentity())
463  {
464  initGravity_ = true;
465  this->reset(this->getPose()*p.rotation());
466  }
467 
468  if(previousPose_.isIdentity())
469  {
470  previousPose_ = p;
471  }
472 
473  // make it incremental
474  t = previousPose_.inverse()*p;
475  previousPose_ = p;
476 
477  if(info)
478  {
479  info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
480  info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
481 
482  // FIXME: the scale of landmarks doesn't seem to fit well the environment...
483  /*info->localMap = okvisCallbackHandler_->getLastLandmarks();
484  info->localMapSize = info->localMap.size();
485  for(std::map<int, cv::Point3f>::iterator iter=info->localMap.begin(); iter!=info->localMap.end(); ++iter)
486  {
487  iter->second = util3d::transformPoint(iter->second, fixPos);
488  }*/
489  }
490  }
491  UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
492  }
493  }
494 #else
495  UERROR("RTAB-Map is not built with OKVIS support! Select another visual odometry approach.");
496 #endif
497  return t;
498 }
499 
500 } // namespace rtabmap
rtabmap::CameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: CameraModel.h:115
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
cam
rtabmap::CameraThread * cam
Definition: tools/DataRecorder/main.cpp:58
UTimer
Definition: UTimer.h:46
rtabmap::Odometry::imagesAlreadyRectified
bool imagesAlreadyRectified() const
Definition: Odometry.h:82
rtabmap::OdometryOkvis::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryOkvis.cpp:185
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
x
x
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
T
T
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
UFATAL
#define UFATAL(...)
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::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
UMutex
Definition: UMutex.h:54
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:371
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
uReplaceChar
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
Definition: UConversion.cpp:33
util3d_transforms.h
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::IMU::angularVelocity
const cv::Vec3d & angularVelocity() const
Definition: IMU.h:56
false
#define false
Definition: ConvertUTF.c:56
rtabmap::OdometryOkvis::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryOkvis.cpp:162
rtabmap::Transform::fromEigen4d
static Transform fromEigen4d(const Eigen::Matrix4d &matrix)
Definition: Transform.cpp:412
UDEBUG
#define UDEBUG(...)
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:67
UFile::exists
bool exists()
Definition: UFile.h:104
dist
dist
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
UWARN
#define UWARN(...)
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
rtabmap::OdometryOkvis::configFilename_
std::string configFilename_
Definition: OdometryOkvis.h:55
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
rtabmap::IMU::linearAcceleration
const cv::Vec3d linearAcceleration() const
Definition: IMU.h:59
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
rtabmap::SensorData::imu
const IMU & imu() const
Definition: SensorData.h:290
rtabmap::OdometryOkvis::OdometryOkvis
OdometryOkvis(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryOkvis.cpp:132
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
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