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/UFile.h"
32 #include "rtabmap/utilite/ULogger.h"
33 #include "rtabmap/utilite/UTimer.h"
34 #include <opencv2/core/eigen.hpp>
35 #include <opencv2/imgproc/types_c.h>
36 
37 #ifdef RTABMAP_OPENVINS
38 #include "core/VioManager.h"
39 #include "state/Propagator.h"
40 #include "state/State.h"
41 #include "state/StateHelper.h"
42 #endif
43 
44 namespace rtabmap {
45 
46 OdometryOpenVINS::OdometryOpenVINS(const ParametersMap & parameters) :
47  Odometry(parameters)
48 #ifdef RTABMAP_OPENVINS
49  ,
50  initGravity_(false),
51  previousPoseInv_(Transform::getIdentity())
52 #endif
53 {
54 #ifdef RTABMAP_OPENVINS
55  ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel(ULogger::level()+1));
56  int enum_index;
57  std::string left_mask_path, right_mask_path;
58  params_ = std::make_unique<ov_msckf::VioManagerOptions>();
59  Parameters::parse(parameters, Parameters::kOdomOpenVINSUseStereo(), params_->use_stereo);
60  Parameters::parse(parameters, Parameters::kOdomOpenVINSUseKLT(), params_->use_klt);
61  Parameters::parse(parameters, Parameters::kOdomOpenVINSNumPts(), params_->num_pts);
62  Parameters::parse(parameters, Parameters::kFASTThreshold(), params_->fast_threshold);
63  Parameters::parse(parameters, Parameters::kVisGridCols(), params_->grid_x);
64  Parameters::parse(parameters, Parameters::kVisGridRows(), params_->grid_y);
65  Parameters::parse(parameters, Parameters::kOdomOpenVINSMinPxDist(), params_->min_px_dist);
66  Parameters::parse(parameters, Parameters::kVisCorNNDR(), params_->knn_ratio);
67  Parameters::parse(parameters, Parameters::kOdomOpenVINSFiTriangulate1d(), params_->featinit_options.triangulate_1d);
68  Parameters::parse(parameters, Parameters::kOdomOpenVINSFiRefineFeatures(), params_->featinit_options.refine_features);
69  Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxRuns(), params_->featinit_options.max_runs);
70  Parameters::parse(parameters, Parameters::kVisMinDepth(), params_->featinit_options.min_dist);
71  Parameters::parse(parameters, Parameters::kVisMaxDepth(), params_->featinit_options.max_dist);
72  if(params_->featinit_options.max_dist == 0)
73  params_->featinit_options.max_dist = std::numeric_limits<double>::infinity();
74  Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxBaseline(), params_->featinit_options.max_baseline);
75  Parameters::parse(parameters, Parameters::kOdomOpenVINSFiMaxCondNumber(), params_->featinit_options.max_cond_number);
76  Parameters::parse(parameters, Parameters::kOdomOpenVINSUseFEJ(), params_->state_options.do_fej);
77  Parameters::parse(parameters, Parameters::kOdomOpenVINSIntegration(), enum_index);
78  params_->state_options.integration_method = ov_msckf::StateOptions::IntegrationMethod(enum_index);
79  Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamExtrinsics(), params_->state_options.do_calib_camera_pose);
80  Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamIntrinsics(), params_->state_options.do_calib_camera_intrinsics);
81  Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibCamTimeoffset(), params_->state_options.do_calib_camera_timeoffset);
82  Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUIntrinsics(), params_->state_options.do_calib_imu_intrinsics);
83  Parameters::parse(parameters, Parameters::kOdomOpenVINSCalibIMUGSensitivity(), params_->state_options.do_calib_imu_g_sensitivity);
84  Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxClones(), params_->state_options.max_clone_size);
85  Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAM(), params_->state_options.max_slam_features);
86  Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxSLAMInUpdate(), params_->state_options.max_slam_in_update);
87  Parameters::parse(parameters, Parameters::kOdomOpenVINSMaxMSCKFInUpdate(), params_->state_options.max_msckf_in_update);
88  Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepMSCKF(), enum_index);
89  params_->state_options.feat_rep_msckf = ov_type::LandmarkRepresentation::Representation(enum_index);
90  Parameters::parse(parameters, Parameters::kOdomOpenVINSFeatRepSLAM(), enum_index);
91  params_->state_options.feat_rep_slam = ov_type::LandmarkRepresentation::Representation(enum_index);
92  Parameters::parse(parameters, Parameters::kOdomOpenVINSDtSLAMDelay(), params_->dt_slam_delay);
93  Parameters::parse(parameters, Parameters::kOdomOpenVINSGravityMag(), params_->gravity_mag);
94  Parameters::parse(parameters, Parameters::kVisDepthAsMask(), params_->use_mask);
95  Parameters::parse(parameters, Parameters::kOdomOpenVINSLeftMaskPath(), left_mask_path);
96  if(!left_mask_path.empty())
97  {
98  if(!UFile::exists(left_mask_path))
99  UWARN("OpenVINS: invalid left mask path: %s", left_mask_path.c_str());
100  else
101  params_->masks.emplace(0, cv::imread(left_mask_path, cv::IMREAD_GRAYSCALE));
102  }
103  Parameters::parse(parameters, Parameters::kOdomOpenVINSRightMaskPath(), right_mask_path);
104  if(!right_mask_path.empty())
105  {
106  if(!UFile::exists(right_mask_path))
107  UWARN("OpenVINS: invalid right mask path: %s", right_mask_path.c_str());
108  else
109  params_->masks.emplace(1, cv::imread(right_mask_path, cv::IMREAD_GRAYSCALE));
110  }
111  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitWindowTime(), params_->init_options.init_window_time);
112  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitIMUThresh(), params_->init_options.init_imu_thresh);
113  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxDisparity(), params_->init_options.init_max_disparity);
114  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitMaxFeatures(), params_->init_options.init_max_features);
115  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynUse(), params_->init_options.init_dyn_use);
116  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEOptCalib(), params_->init_options.init_dyn_mle_opt_calib);
117  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxIter(), params_->init_options.init_dyn_mle_max_iter);
118  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxTime(), params_->init_options.init_dyn_mle_max_time);
119  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMLEMaxThreads(), params_->init_options.init_dyn_mle_max_threads);
120  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynNumPose(), params_->init_options.init_dyn_num_pose);
121  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinDeg(), params_->init_options.init_dyn_min_deg);
122  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationOri(), params_->init_options.init_dyn_inflation_orientation);
123  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationVel(), params_->init_options.init_dyn_inflation_velocity);
124  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBg(), params_->init_options.init_dyn_inflation_bias_gyro);
125  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynInflationBa(), params_->init_options.init_dyn_inflation_bias_accel);
126  Parameters::parse(parameters, Parameters::kOdomOpenVINSInitDynMinRecCond(), params_->init_options.init_dyn_min_rec_cond);
127  Parameters::parse(parameters, Parameters::kOdomOpenVINSTryZUPT(), params_->try_zupt);
128  Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTChi2Multiplier(), params_->zupt_options.chi2_multipler);
129  Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxVelodicy(), params_->zupt_max_velocity);
130  Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTNoiseMultiplier(), params_->zupt_noise_multiplier);
131  Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTMaxDisparity(), params_->zupt_max_disparity);
132  Parameters::parse(parameters, Parameters::kOdomOpenVINSZUPTOnlyAtBeginning(), params_->zupt_only_at_beginning);
133  Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerNoiseDensity(), params_->imu_noises.sigma_a);
134  Parameters::parse(parameters, Parameters::kOdomOpenVINSAccelerometerRandomWalk(), params_->imu_noises.sigma_ab);
135  Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeNoiseDensity(), params_->imu_noises.sigma_w);
136  Parameters::parse(parameters, Parameters::kOdomOpenVINSGyroscopeRandomWalk(), params_->imu_noises.sigma_wb);
137  Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFSigmaPx(), params_->msckf_options.sigma_pix);
138  Parameters::parse(parameters, Parameters::kOdomOpenVINSUpMSCKFChi2Multiplier(), params_->msckf_options.chi2_multipler);
139  Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMSigmaPx(), params_->slam_options.sigma_pix);
140  Parameters::parse(parameters, Parameters::kOdomOpenVINSUpSLAMChi2Multiplier(), params_->slam_options.chi2_multipler);
141  params_->vec_dw << 1, 0, 0, 1, 0, 1;
142  params_->vec_da << 1, 0, 0, 1, 0, 1;
143  params_->vec_tg << 0, 0, 0, 0, 0, 0, 0, 0, 0;
144  params_->q_ACCtoIMU << 0, 0, 0, 1;
145  params_->q_GYROtoIMU << 0, 0, 0, 1;
146  params_->use_aruco = false;
147  params_->num_opencv_threads = -1;
148  params_->histogram_method = ov_core::TrackBase::HistogramMethod::NONE;
149  params_->init_options.sigma_a = params_->imu_noises.sigma_a;
150  params_->init_options.sigma_ab = params_->imu_noises.sigma_ab;
151  params_->init_options.sigma_w = params_->imu_noises.sigma_w;
152  params_->init_options.sigma_wb = params_->imu_noises.sigma_wb;
153  params_->init_options.sigma_pix = params_->slam_options.sigma_pix;
154  params_->init_options.gravity_mag = params_->gravity_mag;
155 #endif
156 }
157 
158 void OdometryOpenVINS::reset(const Transform & initialPose)
159 {
160  Odometry::reset(initialPose);
161 #ifdef RTABMAP_OPENVINS
162  if(!initGravity_)
163  {
164  vioManager_.reset();
165  previousPoseInv_.setIdentity();
166  imuLocalTransformInv_.setNull();
167  }
168  initGravity_ = false;
169 #endif
170 }
171 
172 // return not null transform if odometry is correctly computed
173 Transform OdometryOpenVINS::computeTransform(
174  SensorData & data,
175  const Transform & guess,
176  OdometryInfo * info)
177 {
178  Transform t;
179 #ifdef RTABMAP_OPENVINS
180 
181  if(!vioManager_)
182  {
183  if(!data.imu().empty())
184  {
185  imuLocalTransformInv_ = data.imu().localTransform().inverse();
186  Phi_.setZero();
187  Phi_.block(0,0,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3);
188  Phi_.block(3,3,3,3) = data.imu().localTransform().toEigen4d().block(0,0,3,3);
189  }
190 
191  if(!data.imageRaw().empty() && !imuLocalTransformInv_.isNull())
192  {
193  Transform T_imu_left;
194  Eigen::VectorXd left_calib(8), right_calib(8);
195  if(!data.rightRaw().empty())
196  {
197  params_->state_options.num_cameras = params_->init_options.num_cameras = 2;
198  T_imu_left = imuLocalTransformInv_ * data.stereoCameraModels()[0].localTransform();
199 
200  bool is_fisheye = data.stereoCameraModels()[0].left().isFisheye() && !this->imagesAlreadyRectified();
201  if(is_fisheye)
202  {
203  params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
204  data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
205  params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamEqui>(
206  data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
207  }
208  else
209  {
210  params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
211  data.stereoCameraModels()[0].left().imageWidth(), data.stereoCameraModels()[0].left().imageHeight()));
212  params_->camera_intrinsics.emplace(1, std::make_shared<ov_core::CamRadtan>(
213  data.stereoCameraModels()[0].right().imageWidth(), data.stereoCameraModels()[0].right().imageHeight()));
214  }
215 
216  if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].left().D_raw().empty())
217  {
218  left_calib << data.stereoCameraModels()[0].left().fx(),
219  data.stereoCameraModels()[0].left().fy(),
220  data.stereoCameraModels()[0].left().cx(),
221  data.stereoCameraModels()[0].left().cy(), 0, 0, 0, 0;
222  right_calib << data.stereoCameraModels()[0].right().fx(),
223  data.stereoCameraModels()[0].right().fy(),
224  data.stereoCameraModels()[0].right().cx(),
225  data.stereoCameraModels()[0].right().cy(), 0, 0, 0, 0;
226  }
227  else
228  {
229  UASSERT(data.stereoCameraModels()[0].left().D_raw().cols == data.stereoCameraModels()[0].right().D_raw().cols);
230  UASSERT(data.stereoCameraModels()[0].left().D_raw().cols >= 4);
231  UASSERT(data.stereoCameraModels()[0].right().D_raw().cols >= 4);
232  left_calib << data.stereoCameraModels()[0].left().K_raw().at<double>(0,0),
233  data.stereoCameraModels()[0].left().K_raw().at<double>(1,1),
234  data.stereoCameraModels()[0].left().K_raw().at<double>(0,2),
235  data.stereoCameraModels()[0].left().K_raw().at<double>(1,2),
236  data.stereoCameraModels()[0].left().D_raw().at<double>(0,0),
237  data.stereoCameraModels()[0].left().D_raw().at<double>(0,1),
238  data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?4:2),
239  data.stereoCameraModels()[0].left().D_raw().at<double>(0,is_fisheye?5:3);
240  right_calib << data.stereoCameraModels()[0].right().K_raw().at<double>(0,0),
241  data.stereoCameraModels()[0].right().K_raw().at<double>(1,1),
242  data.stereoCameraModels()[0].right().K_raw().at<double>(0,2),
243  data.stereoCameraModels()[0].right().K_raw().at<double>(1,2),
244  data.stereoCameraModels()[0].right().D_raw().at<double>(0,0),
245  data.stereoCameraModels()[0].right().D_raw().at<double>(0,1),
246  data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?4:2),
247  data.stereoCameraModels()[0].right().D_raw().at<double>(0,is_fisheye?5:3);
248  }
249  }
250  else
251  {
252  params_->state_options.num_cameras = params_->init_options.num_cameras = 1;
253  T_imu_left = imuLocalTransformInv_ * data.cameraModels()[0].localTransform();
254 
255  bool is_fisheye = data.cameraModels()[0].isFisheye() && !this->imagesAlreadyRectified();
256  if(is_fisheye)
257  {
258  params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamEqui>(
259  data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
260  }
261  else
262  {
263  params_->camera_intrinsics.emplace(0, std::make_shared<ov_core::CamRadtan>(
264  data.cameraModels()[0].imageWidth(), data.cameraModels()[0].imageHeight()));
265  }
266 
267  if(this->imagesAlreadyRectified() || data.cameraModels()[0].D_raw().empty())
268  {
269  left_calib << data.cameraModels()[0].fx(),
270  data.cameraModels()[0].fy(),
271  data.cameraModels()[0].cx(),
272  data.cameraModels()[0].cy(), 0, 0, 0, 0;
273  }
274  else
275  {
276  UASSERT(data.cameraModels()[0].D_raw().cols >= 4);
277  left_calib << data.cameraModels()[0].K_raw().at<double>(0,0),
278  data.cameraModels()[0].K_raw().at<double>(1,1),
279  data.cameraModels()[0].K_raw().at<double>(0,2),
280  data.cameraModels()[0].K_raw().at<double>(1,2),
281  data.cameraModels()[0].D_raw().at<double>(0,0),
282  data.cameraModels()[0].D_raw().at<double>(0,1),
283  data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?4:2),
284  data.cameraModels()[0].D_raw().at<double>(0,is_fisheye?5:3);
285  }
286  }
287 
288  Eigen::Matrix4d T_LtoI = T_imu_left.toEigen4d();
289  Eigen::Matrix<double,7,1> left_eigen;
290  left_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_LtoI.block(0,0,3,3).transpose());
291  left_eigen.block(4,0,3,1) = -T_LtoI.block(0,0,3,3).transpose()*T_LtoI.block(0,3,3,1);
292  params_->camera_intrinsics.at(0)->set_value(left_calib);
293  params_->camera_extrinsics.emplace(0, left_eigen);
294  if(!data.rightRaw().empty())
295  {
296  Transform T_left_right;
297  if(this->imagesAlreadyRectified() || data.stereoCameraModels()[0].stereoTransform().isNull())
298  {
299  T_left_right = Transform(
300  1, 0, 0, data.stereoCameraModels()[0].baseline(),
301  0, 1, 0, 0,
302  0, 0, 1, 0);
303  }
304  else
305  {
306  T_left_right = data.stereoCameraModels()[0].stereoTransform().inverse();
307  }
308  UASSERT(!T_left_right.isNull());
309  Transform T_imu_right = T_imu_left * T_left_right;
310  Eigen::Matrix4d T_RtoI = T_imu_right.toEigen4d();
311  Eigen::Matrix<double,7,1> right_eigen;
312  right_eigen.block(0,0,4,1) = ov_core::rot_2_quat(T_RtoI.block(0,0,3,3).transpose());
313  right_eigen.block(4,0,3,1) = -T_RtoI.block(0,0,3,3).transpose()*T_RtoI.block(0,3,3,1);
314  params_->camera_intrinsics.at(1)->set_value(right_calib);
315  params_->camera_extrinsics.emplace(1, right_eigen);
316  }
317  params_->init_options.camera_intrinsics = params_->camera_intrinsics;
318  params_->init_options.camera_extrinsics = params_->camera_extrinsics;
319  vioManager_ = std::make_unique<ov_msckf::VioManager>(*params_);
320  }
321  }
322  else
323  {
324  if(!data.imu().empty())
325  {
326  ov_core::ImuData message;
327  message.timestamp = data.stamp();
328  message.wm << data.imu().angularVelocity().val[0], data.imu().angularVelocity().val[1], data.imu().angularVelocity().val[2];
329  message.am << data.imu().linearAcceleration().val[0], data.imu().linearAcceleration().val[1], data.imu().linearAcceleration().val[2];
330  vioManager_->feed_measurement_imu(message);
331  }
332 
333  if(!data.imageRaw().empty())
334  {
335  bool covFilled = false;
336  Eigen::Matrix<double, 13, 1> state_plus = Eigen::Matrix<double, 13, 1>::Zero();
337  Eigen::Matrix<double, 12, 12> cov_plus = Eigen::Matrix<double, 12, 12>::Zero();
338  if(vioManager_->initialized())
339  covFilled = vioManager_->get_propagator()->fast_state_propagate(vioManager_->get_state(), data.stamp(), state_plus, cov_plus);
340 
341  cv::Mat image;
342  if(data.imageRaw().type() == CV_8UC3)
343  cv::cvtColor(data.imageRaw(), image, CV_BGR2GRAY);
344  else if(data.imageRaw().type() == CV_8UC1)
345  image = data.imageRaw().clone();
346  else
347  UFATAL("Not supported color type!");
348  ov_core::CameraData message;
349  message.timestamp = data.stamp();
350  message.sensor_ids.emplace_back(0);
351  message.images.emplace_back(image);
352  if(params_->masks.find(0) != params_->masks.end())
353  {
354  message.masks.emplace_back(params_->masks[0]);
355  }
356  else if(!data.depthRaw().empty() && params_->use_mask)
357  {
358  cv::Mat mask;
359  if(data.depthRaw().type() == CV_32FC1)
360  cv::inRange(data.depthRaw(), params_->featinit_options.min_dist,
361  std::isinf(params_->featinit_options.max_dist)?std::numeric_limits<float>::max():params_->featinit_options.max_dist, mask);
362  else if(data.depthRaw().type() == CV_16UC1)
363  cv::inRange(data.depthRaw(), params_->featinit_options.min_dist*1000,
364  std::isinf(params_->featinit_options.max_dist)?std::numeric_limits<uint16_t>::max():params_->featinit_options.max_dist*1000, mask);
365  message.masks.emplace_back(255-mask);
366  }
367  else
368  {
369  message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
370  }
371  if(!data.rightRaw().empty())
372  {
373  if(data.rightRaw().type() == CV_8UC3)
374  cv::cvtColor(data.rightRaw(), image, CV_BGR2GRAY);
375  else if(data.rightRaw().type() == CV_8UC1)
376  image = data.rightRaw().clone();
377  else
378  UFATAL("Not supported color type!");
379  message.sensor_ids.emplace_back(1);
380  message.images.emplace_back(image);
381  if(params_->masks.find(1) != params_->masks.end())
382  message.masks.emplace_back(params_->masks[1]);
383  else
384  message.masks.emplace_back(cv::Mat::zeros(image.size(), CV_8UC1));
385  }
386  vioManager_->feed_measurement_camera(message);
387 
388  std::shared_ptr<ov_msckf::State> state = vioManager_->get_state();
389  Transform p((float)state->_imu->pos()(0),
390  (float)state->_imu->pos()(1),
391  (float)state->_imu->pos()(2),
392  (float)state->_imu->quat()(0),
393  (float)state->_imu->quat()(1),
394  (float)state->_imu->quat()(2),
395  (float)state->_imu->quat()(3));
396  if(!p.isNull() && !p.isIdentity())
397  {
398  p = p * imuLocalTransformInv_;
399 
400  if(this->getPose().rotation().isIdentity())
401  {
402  initGravity_ = true;
403  this->reset(this->getPose() * p.rotation());
404  }
405 
406  if(previousPoseInv_.isIdentity())
407  previousPoseInv_ = p.inverse();
408 
409  t = previousPoseInv_ * p;
410 
411  if(info)
412  {
413  double timestamp;
414  std::unordered_map<size_t, Eigen::Vector3d> feat_posinG, feat_tracks_uvd;
415  vioManager_->get_active_tracks(timestamp, feat_posinG, feat_tracks_uvd);
416  auto features_SLAM = vioManager_->get_features_SLAM();
417  auto good_features_MSCKF = vioManager_->get_good_features_MSCKF();
418 
419  info->type = this->getType();
420  info->localMapSize = feat_posinG.size();
421  info->features = features_SLAM.size() + good_features_MSCKF.size();
422  info->reg.covariance = cv::Mat::eye(6, 6, CV_64FC1);
423  if(covFilled)
424  {
425  Eigen::Matrix<double, 6, 6> covariance = Phi_ * cov_plus.block(6,6,6,6) * Phi_.transpose();
426  cv::eigen2cv(covariance, info->reg.covariance);
427  }
428 
429  if(this->isInfoDataFilled())
430  {
431  Transform fixT = this->getPose() * previousPoseInv_;
432  Transform camT;
433  if(!data.rightRaw().empty())
434  camT = data.stereoCameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT;
435  else
436  camT = data.cameraModels()[0].localTransform().inverse() * t.inverse() * this->getPose().inverse() * fixT;
437 
438  for(auto &feature : feat_posinG)
439  {
440  cv::Point3f pt3d(feature.second[0], feature.second[1], feature.second[2]);
441  pt3d = util3d::transformPoint(pt3d, fixT);
442  info->localMap.emplace(feature.first, pt3d);
443  }
444 
445  if(this->imagesAlreadyRectified())
446  {
447  for(auto &feature : features_SLAM)
448  {
449  cv::Point3f pt3d(feature[0], feature[1], feature[2]);
450  pt3d = util3d::transformPoint(pt3d, camT);
451  cv::Point2f pt;
452  if(!data.rightRaw().empty())
453  data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
454  else
455  data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
456  info->reg.inliersIDs.emplace_back(info->newCorners.size());
457  info->newCorners.emplace_back(pt);
458  }
459 
460  for(auto &feature : good_features_MSCKF)
461  {
462  cv::Point3f pt3d(feature[0], feature[1], feature[2]);
463  pt3d = util3d::transformPoint(pt3d, camT);
464  cv::Point2f pt;
465  if(!data.rightRaw().empty())
466  data.stereoCameraModels()[0].left().reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
467  else
468  data.cameraModels()[0].reproject(pt3d.x, pt3d.y, pt3d.z, pt.x, pt.y);
469  info->reg.matchesIDs.emplace_back(info->newCorners.size());
470  info->newCorners.emplace_back(pt);
471  }
472  }
473  }
474  }
475 
476  previousPoseInv_ = p.inverse();
477  }
478  }
479  }
480 
481 #else
482  UERROR("RTAB-Map is not built with OpenVINS support! Select another visual odometry approach.");
483 #endif
484  return t;
485 }
486 
487 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::OdometryInfo::localMapSize
int localMapSize
Definition: OdometryInfo.h:101
glm::mask
GLM_FUNC_DECL genIType mask(genIType const &count)
OdometryInfo.h
OdometryOpenVINS.h
rtabmap::OdometryOpenVINS::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryOpenVINS.cpp:158
rtabmap::OdometryOpenVINS::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryOpenVINS.cpp:173
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::OdometryOpenVINS::getType
virtual Odometry::Type getType()
Definition: OdometryOpenVINS.h:46
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
glm::isinf
GLM_FUNC_DECL genType::bool_type isinf(genType const &x)
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
UFATAL
#define UFATAL(...)
data
int data[]
util3d_transforms.h
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
info
else if n * info
UASSERT
#define UASSERT(condition)
p
Point3_ p(2)
rtabmap::Transform::toEigen4d
Eigen::Matrix4d toEigen4d() const
Definition: Transform.cpp:381
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UWARN
#define UWARN(...)
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
ULogger::level
static ULogger::Level level()
Definition: ULogger.h:340
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
rtabmap::OdometryOpenVINS::OdometryOpenVINS
OdometryOpenVINS(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryOpenVINS.cpp:46
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Odometry
Definition: Odometry.h:42
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
Eigen::Matrix
false
#define false
Definition: ConvertUTF.c:56
t
Point2 t(10, 10)
rtabmap::Odometry::imagesAlreadyRectified
bool imagesAlreadyRectified() const
Definition: Odometry.h:83
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
UERROR
#define UERROR(...)
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:13