OdometryOpenVINS.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/OdometryOpenVINS.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_OPENVINS
39 #include "core/VioManager.h"
40 #include "core/VioManagerOptions.h"
41 #include "core/RosVisualizer.h"
42 #include "utils/dataset_reader.h"
43 #include "utils/parse_ros.h"
44 #include "utils/sensor_data.h"
45 #include "state/State.h"
46 #include "types/Type.h"
47 #endif
48 
49 namespace rtabmap {
50 
51 OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
52  Odometry(parameters)
53 #ifdef RTABMAP_OPENVINS
54  ,
55  vioManager_(0),
56  initGravity_(false),
57  previousPose_(Transform::getIdentity())
58 #endif
59 {
60 }
61 
62 OdometryOpenVINS::~OdometryOpenVINS()
63 {
64 #ifdef RTABMAP_OPENVINS
65  delete vioManager_;
66 #endif
67 }
68 
69 void OdometryOpenVINS::reset(const Transform & initialPose)
70 {
71  Odometry::reset(initialPose);
72 #ifdef RTABMAP_OPENVINS
73  if(!initGravity_)
74  {
75  delete vioManager_;
76  vioManager_ = 0;
77  previousPose_.setIdentity();
78  previousLocalTransform_.setNull();
79  imuBuffer_.clear();
80  }
81  initGravity_ = false;
82 #endif
83 }
84 
85 // return not null transform if odometry is correctly computed
86 Transform OdometryOpenVINS::computeTransform(
87  SensorData & data,
88  const Transform & guess,
89  OdometryInfo * info)
90 {
91  Transform t;
92 #ifdef RTABMAP_OPENVINS
93  UTimer timer;
94 
95  // Buffer imus;
96  if(!data.imu().empty())
97  {
98  imuBuffer_.insert(std::make_pair(data.stamp(), data.imu()));
99  }
100 
101  // OpenVINS has to buffer image before computing transformation with IMU stamp > image stamp
102  if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1)
103  {
104  if(imuBuffer_.empty())
105  {
106  UWARN("Waiting IMU for initialization...");
107  return t;
108  }
109  if(vioManager_ == 0)
110  {
111  UINFO("OpenVINS Initialization");
112 
113  // intialize
114  ov_msckf::VioManagerOptions params;
115 
116  // ESTIMATOR ======================================================================
117 
118  // Main EKF parameters
119  //params.state_options.do_fej = true;
120  //params.state_options.imu_avg =false;
121  //params.state_options.use_rk4_integration;
122  //params.state_options.do_calib_camera_pose = false;
123  //params.state_options.do_calib_camera_intrinsics = false;
124  //params.state_options.do_calib_camera_timeoffset = false;
125  //params.state_options.max_clone_size = 11;
126  //params.state_options.max_slam_features = 25;
127  //params.state_options.max_slam_in_update = INT_MAX;
128  //params.state_options.max_msckf_in_update = INT_MAX;
129  //params.state_options.max_aruco_features = 1024;
130  params.state_options.num_cameras = 2;
131  //params.dt_slam_delay = 2;
132 
133  // Set what representation we should be using
134  //params.state_options.feat_rep_msckf = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
135  //params.state_options.feat_rep_slam = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
136  //params.state_options.feat_rep_aruco = LandmarkRepresentation::from_string("ANCHORED_MSCKF_INVERSE_DEPTH"); // default GLOBAL_3D
137  if( params.state_options.feat_rep_msckf == LandmarkRepresentation::Representation::UNKNOWN ||
138  params.state_options.feat_rep_slam == LandmarkRepresentation::Representation::UNKNOWN ||
139  params.state_options.feat_rep_aruco == LandmarkRepresentation::Representation::UNKNOWN)
140  {
141  printf(RED "VioManager(): invalid feature representation specified:\n" RESET);
142  printf(RED "\t- GLOBAL_3D\n" RESET);
143  printf(RED "\t- GLOBAL_FULL_INVERSE_DEPTH\n" RESET);
144  printf(RED "\t- ANCHORED_3D\n" RESET);
145  printf(RED "\t- ANCHORED_FULL_INVERSE_DEPTH\n" RESET);
146  printf(RED "\t- ANCHORED_MSCKF_INVERSE_DEPTH\n" RESET);
147  printf(RED "\t- ANCHORED_INVERSE_DEPTH_SINGLE\n" RESET);
148  std::exit(EXIT_FAILURE);
149  }
150 
151  // Filter initialization
152  //params.init_window_time = 1;
153  //params.init_imu_thresh = 1;
154 
155  // Zero velocity update
156  //params.try_zupt = false;
157  //params.zupt_options.chi2_multipler = 5;
158  //params.zupt_max_velocity = 1;
159  //params.zupt_noise_multiplier = 1;
160 
161  // NOISE ======================================================================
162 
163  // Our noise values for inertial sensor
164  //params.imu_noises.sigma_w = 1.6968e-04;
165  //params.imu_noises.sigma_a = 2.0000e-3;
166  //params.imu_noises.sigma_wb = 1.9393e-05;
167  //params.imu_noises.sigma_ab = 3.0000e-03;
168 
169  // Read in update parameters
170  //params.msckf_options.sigma_pix = 1;
171  //params.msckf_options.chi2_multipler = 5;
172  //params.slam_options.sigma_pix = 1;
173  //params.slam_options.chi2_multipler = 5;
174  //params.aruco_options.sigma_pix = 1;
175  //params.aruco_options.chi2_multipler = 5;
176 
177 
178  // STATE ======================================================================
179 
180  // Timeoffset from camera to IMU
181  //params.calib_camimu_dt = 0.0;
182 
183  // Global gravity
184  //params.gravity[2] = 9.81;
185 
186 
187  // TRACKERS ======================================================================
188 
189  // Tracking flags
190  params.use_stereo = true;
191  //params.use_klt = true;
192  params.use_aruco = false;
193  //params.downsize_aruco = true;
194  //params.downsample_cameras = false;
195  //params.use_multi_threading = true;
196 
197  // General parameters
198  //params.num_pts = 200;
199  //params.fast_threshold = 10;
200  //params.grid_x = 10;
201  //params.grid_y = 5;
202  //params.min_px_dist = 8;
203  //params.knn_ratio = 0.7;
204 
205  // Feature initializer parameters
206  //nh.param<bool>("fi_triangulate_1d", params.featinit_options.triangulate_1d, params.featinit_options.triangulate_1d);
207  //nh.param<bool>("fi_refine_features", params.featinit_options.refine_features, params.featinit_options.refine_features);
208  //nh.param<int>("fi_max_runs", params.featinit_options.max_runs, params.featinit_options.max_runs);
209  //nh.param<double>("fi_init_lamda", params.featinit_options.init_lamda, params.featinit_options.init_lamda);
210  //nh.param<double>("fi_max_lamda", params.featinit_options.max_lamda, params.featinit_options.max_lamda);
211  //nh.param<double>("fi_min_dx", params.featinit_options.min_dx, params.featinit_options.min_dx);
213  //nh.param<double>("fi_lam_mult", params.featinit_options.lam_mult, params.featinit_options.lam_mult);
214  //nh.param<double>("fi_min_dist", params.featinit_options.min_dist, params.featinit_options.min_dist);
215  //params.featinit_options.max_dist = 75;
216  //params.featinit_options.max_baseline = 500;
217  //params.featinit_options.max_cond_number = 5000;
218 
219 
220  // CAMERA ======================================================================
221  bool fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
222  params.camera_fisheye.insert(std::make_pair(0, fisheye));
223  params.camera_fisheye.insert(std::make_pair(1, fisheye));
224 
225  Eigen::VectorXd camLeft(8), camRight(8);
226  if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
227  {
228  camLeft << data.stereoCameraModels()[0].left().fx(),
229  data.stereoCameraModels()[0].left().fy(),
230  data.stereoCameraModels()[0].left().cx(),
231  data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
232  camRight << data.stereoCameraModels()[0].right().fx(),
233  data.stereoCameraModels()[0].right().fy(),
234  data.stereoCameraModels()[0].right().cx(),
235  data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
236  }
237  else
238  {
239  UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
240  UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
241  UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
242 
243  //https://github.com/ethz-asl/kalibr/wiki/supported-models
245  // (distortion_coeffs: [k1 k2 r1 r2])
247  // (distortion_coeffs: [k1 k2 k3 k4]) rtabmap: (k1,k2,p1,p2,k3,k4)
248 
249  camLeft <<
250  data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
251  data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
252  data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
253  data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
254  data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
255  data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
256  data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?4:2),
257  data.stereoCameraModels()[0].left().D_raw().at<double>(0,fisheye?5:3);
258  camRight <<
259  data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
260  data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
261  data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
262  data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
263  data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
264  data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
265  data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?4:2),
266  data.stereoCameraModels()[0].right().D_raw().at<double>(0,fisheye?5:3);
267  }
268  params.camera_intrinsics.insert(std::make_pair(0, camLeft));
269  params.camera_intrinsics.insert(std::make_pair(1, camRight));
270 
271  const IMU & imu = imuBuffer_.begin()->second;
272  imuLocalTransform_ = imu.localTransform();
273  Transform imuCam0 = imuLocalTransform_.inverse() * data.stereoCameraModels()[0].localTransform();
274  Transform cam0cam1;
275  if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
276  {
277  cam0cam1 = Transform(
278  1, 0, 0, data.stereoCameraModels()[0].baseline(),
279  0, 1, 0, 0,
280  0, 0, 1, 0);
281  }
282  else
283  {
284  cam0cam1 = data.stereoCameraModels()[0].stereoTransform().inverse();
285  }
286  UASSERT(!cam0cam1.isNull());
287  Transform imuCam1 = imuCam0 * cam0cam1;
288  Eigen::Matrix4d cam0_eigen = imuCam0.toEigen4d();
289  Eigen::Matrix4d cam1_eigen = imuCam1.toEigen4d();
290  Eigen::Matrix<double,7,1> cam_eigen0;
291  cam_eigen0.block(0,0,4,1) = rot_2_quat(cam0_eigen.block(0,0,3,3).transpose());
292  cam_eigen0.block(4,0,3,1) = -cam0_eigen.block(0,0,3,3).transpose()*cam0_eigen.block(0,3,3,1);
293  Eigen::Matrix<double,7,1> cam_eigen1;
294  cam_eigen1.block(0,0,4,1) = rot_2_quat(cam1_eigen.block(0,0,3,3).transpose());
295  cam_eigen1.block(4,0,3,1) = -cam1_eigen.block(0,0,3,3).transpose()*cam1_eigen.block(0,3,3,1);
296  params.camera_extrinsics.insert(std::make_pair(0, cam_eigen0));
297  params.camera_extrinsics.insert(std::make_pair(1, cam_eigen1));
298 
299  params.camera_wh.insert({0, std::make_pair(data.stereoCameraModels()[0].left().imageWidth(),data.stereoCameraModels()[0].left().imageHeight())});
300  params.camera_wh.insert({1, std::make_pair(data.stereoCameraModels()[0].right().imageWidth(),data.stereoCameraModels()[0].right().imageHeight())});
301 
302  vioManager_ = new ov_msckf::VioManager(params);
303  }
304 
305  cv::Mat left;
306  cv::Mat right;
307  if(data.imageRaw().type() == CV_8UC3)
308  {
309  cv::cvtColor(data.imageRaw(), left, CV_BGR2GRAY);
310  }
311  else if(data.imageRaw().type() == CV_8UC1)
312  {
313  left = data.imageRaw().clone();
314  }
315  else
316  {
317  UFATAL("Not supported color type!");
318  }
319  if(data.rightRaw().type() == CV_8UC3)
320  {
321  cv::cvtColor(data.rightRaw(), right, CV_BGR2GRAY);
322  }
323  else if(data.rightRaw().type() == CV_8UC1)
324  {
325  right = data.rightRaw().clone();
326  }
327  else
328  {
329  UFATAL("Not supported color type!");
330  }
331 
332  // Create the measurement
333  ov_core::CameraData message;
334  message.timestamp = data.stamp();
335  message.sensor_ids.push_back(0);
336  message.sensor_ids.push_back(1);
337  message.images.push_back(left);
338  message.images.push_back(right);
339  message.masks.push_back(cv::Mat::zeros(left.size(), CV_8UC1));
340  message.masks.push_back(cv::Mat::zeros(right.size(), CV_8UC1));
341 
342  // send it to our VIO system
343  vioManager_->feed_measurement_camera(message);
344  UDEBUG("Image update stamp=%f", data.stamp());
345 
346  double lastIMUstamp = 0.0;
347  while(!imuBuffer_.empty())
348  {
349  std::map<double, IMU>::iterator iter = imuBuffer_.begin();
350 
351  // Process IMU data until stamp is over image stamp
352  ov_core::ImuData message;
353  message.timestamp = iter->first;
354  message.wm << iter->second.angularVelocity().val[0], iter->second.angularVelocity().val[1], iter->second.angularVelocity().val[2];
355  message.am << iter->second.linearAcceleration().val[0], iter->second.linearAcceleration().val[1], iter->second.linearAcceleration().val[2];
356 
357  UDEBUG("IMU update stamp=%f", message.timestamp);
358 
359  // send it to our VIO system
360  vioManager_->feed_measurement_imu(message);
361 
362  lastIMUstamp = iter->first;
363 
364  imuBuffer_.erase(iter);
365 
366  if(lastIMUstamp > data.stamp())
367  {
368  break;
369  }
370  }
371 
372  if(vioManager_->initialized())
373  {
374  // Get the current state
375  std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
376 
377  if(state->_timestamp != data.stamp())
378  {
379  UWARN("OpenVINS: Stamp of the current state %f is not the same "
380  "than last image processed %f (last IMU stamp=%f). There could be "
381  "a synchronization issue between camera and IMU. ",
382  state->_timestamp,
383  data.stamp(),
384  lastIMUstamp);
385  }
386 
387  Transform p(
388  (float)state->_imu->pos()(0),
389  (float)state->_imu->pos()(1),
390  (float)state->_imu->pos()(2),
391  (float)state->_imu->quat()(0),
392  (float)state->_imu->quat()(1),
393  (float)state->_imu->quat()(2),
394  (float)state->_imu->quat()(3));
395 
396 
397  // Finally set the covariance in the message (in the order position then orientation as per ros convention)
398  std::vector<std::shared_ptr<ov_type::Type>> statevars;
399  statevars.push_back(state->_imu->pose()->p());
400  statevars.push_back(state->_imu->pose()->q());
401 
402  cv::Mat covariance = cv::Mat::eye(6,6, CV_64FC1);
403  if(this->framesProcessed() == 0)
404  {
405  covariance *= 9999;
406  }
407  else
408  {
409  Eigen::Matrix<double,6,6> covariance_posori = ov_msckf::StateHelper::get_marginal_covariance(vioManager_->get_state(),statevars);
410  for(int r=0; r<6; r++) {
411  for(int c=0; c<6; c++) {
412  ((double *)covariance.data)[6*r+c] = covariance_posori(r,c);
413  }
414  }
415  }
416 
417  if(!p.isNull())
418  {
419  p = p * imuLocalTransform_.inverse();
420 
421  if(this->getPose().rotation().isIdentity())
422  {
423  initGravity_ = true;
424  this->reset(this->getPose()*p.rotation());
425  }
426 
427  if(previousPose_.isIdentity())
428  {
429  previousPose_ = p;
430  }
431 
432  // make it incremental
433  Transform previousPoseInv = previousPose_.inverse();
434  t = previousPoseInv*p;
435  previousPose_ = p;
436 
437  if(info)
438  {
439  info->type = this->getType();
440  info->reg.covariance = covariance;
441 
442  // feature map
443  Transform fixT = this->getPose()*previousPoseInv;
444  Transform camLocalTransformInv = data.stereoCameraModels()[0].localTransform().inverse()*this->getPose().inverse();
445  for (auto &it_per_id : vioManager_->get_features_SLAM())
446  {
447  cv::Point3f pt3d;
448  pt3d.x = it_per_id[0];
449  pt3d.y = it_per_id[1];
450  pt3d.z = it_per_id[2];
451  pt3d = util3d::transformPoint(pt3d, fixT);
452  info->localMap.insert(std::make_pair(info->localMap.size(), pt3d));
453 
454  if(this->imagesAlreadyRectified())
455  {
456  cv::Point2f pt;
457  pt3d = util3d::transformPoint(pt3d, camLocalTransformInv);
458  data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
459  info->reg.inliersIDs.push_back(info->newCorners.size());
460  info->newCorners.push_back(pt);
461  }
462  }
463  info->features = info->newCorners.size();
464  info->localMapSize = info->localMap.size();
465  }
466  UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
467  }
468  }
469  }
470  else if(!data.imageRaw().empty() && !data.depthRaw().empty())
471  {
472  UERROR("OpenVINS doesn't work with RGB-D data, stereo images are required!");
473  }
474  else if(!data.imageRaw().empty() && data.depthOrRightRaw().empty())
475  {
476  UERROR("OpenVINS requires stereo images!");
477  }
478  else
479  {
480  UERROR("OpenVINS requires stereo images (only one stereo camera and should be calibrated)!");
481  }
482 
483 #else
484  UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach.");
485 #endif
486  return t;
487 }
488 
489 } // namespace rtabmap
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::OdometryOpenVINS::OdometryOpenVINS
OdometryOpenVINS(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryOpenVINS.cpp:51
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
RED
RED
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:126
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::OdometryOpenVINS::getType
virtual Odometry::Type getType()
Definition: OdometryOpenVINS.h:46
UFATAL
#define UFATAL(...)
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:211
rtabmap::OdometryOpenVINS::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryOpenVINS.cpp:86
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
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:371
util3d_transforms.h
params
params
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::IMU::localTransform
const Transform & localTransform() const
Definition: IMU.h:62
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
rtabmap::OdometryOpenVINS::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryOpenVINS.cpp:69
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
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::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:131
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:97
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