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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32