OdometryMSCKF.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/OdometryMSCKF.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 <opencv2/imgproc/types_c.h>
36 
37 #ifdef RTABMAP_MSCKF_VIO
38 #include <msckf_vio/image_processor.h>
39 #include <msckf_vio/msckf_vio.h>
40 #include <msckf_vio/math_utils.hpp>
41 #include <eigen_conversions/eigen_msg.h>
42 #include <boost/math/distributions/chi_squared.hpp>
43 #include <pcl/common/transforms.h>
44 #endif
45 
46 namespace rtabmap {
47 
48 #ifdef RTABMAP_MSCKF_VIO
49 class ImageProcessorNoROS: public msckf_vio::ImageProcessor
50 {
51 public:
52  ImageProcessorNoROS(
53  const ParametersMap & parameters_in,
54  const Transform & imuLocalTransform,
55  const StereoCameraModel & model,
56  bool rectified) :
57  msckf_vio::ImageProcessor(0)
58  {
59  UDEBUG("");
60  // Camera calibration parameters
61  if(model.left().D_raw().cols == 6)
62  {
63  //equidistant
64  cam0_distortion_model = "equidistant";
65  cam0_distortion_coeffs[0] = rectified?0:model.left().D_raw().at<double>(0,0);
66  cam0_distortion_coeffs[1] = rectified?0:model.left().D_raw().at<double>(0,1);
67  cam0_distortion_coeffs[2] = rectified?0:model.left().D_raw().at<double>(0,4);
68  cam0_distortion_coeffs[3] = rectified?0:model.left().D_raw().at<double>(0,5);
69  }
70  else
71  {
72  //radtan
73  cam0_distortion_model = "radtan";
74  cam0_distortion_coeffs[0] = rectified?0:model.left().D_raw().at<double>(0,0);
75  cam0_distortion_coeffs[1] = rectified?0:model.left().D_raw().at<double>(0,1);
76  cam0_distortion_coeffs[2] = rectified?0:model.left().D_raw().at<double>(0,2);
77  cam0_distortion_coeffs[3] = rectified?0:model.left().D_raw().at<double>(0,3);
78  }
79  if(model.right().D_raw().cols == 6)
80  {
81  //equidistant
82  cam1_distortion_model = "equidistant";
83  cam1_distortion_coeffs[0] = rectified?0:model.right().D_raw().at<double>(0,0);
84  cam1_distortion_coeffs[1] = rectified?0:model.right().D_raw().at<double>(0,1);
85  cam1_distortion_coeffs[2] = rectified?0:model.right().D_raw().at<double>(0,4);
86  cam1_distortion_coeffs[3] = rectified?0:model.right().D_raw().at<double>(0,5);
87  }
88  else
89  {
90  //radtan
91  cam1_distortion_model = "radtan";
92  cam1_distortion_coeffs[0] = rectified?0:model.right().D_raw().at<double>(0,0);
93  cam1_distortion_coeffs[1] = rectified?0:model.right().D_raw().at<double>(0,1);
94  cam1_distortion_coeffs[2] = rectified?0:model.right().D_raw().at<double>(0,2);
95  cam1_distortion_coeffs[3] = rectified?0:model.right().D_raw().at<double>(0,3);
96  }
97 
98  cam0_resolution[0] = model.left().imageWidth();
99  cam0_resolution[1] = model.left().imageHeight();
100 
101  cam1_resolution[0] = model.right().imageWidth();
102  cam1_resolution[1] = model.right().imageHeight();
103 
104  cam0_intrinsics[0] = rectified?model.left().fx():model.left().K_raw().at<double>(0,0);
105  cam0_intrinsics[1] = rectified?model.left().fy():model.left().K_raw().at<double>(1,1);
106  cam0_intrinsics[2] = rectified?model.left().cx():model.left().K_raw().at<double>(0,2);
107  cam0_intrinsics[3] = rectified?model.left().cy():model.left().K_raw().at<double>(1,2);
108 
109  cam1_intrinsics[0] = rectified?model.right().fx():model.right().K_raw().at<double>(0,0);
110  cam1_intrinsics[1] = rectified?model.right().fy():model.right().K_raw().at<double>(1,1);
111  cam1_intrinsics[2] = rectified?model.right().cx():model.right().K_raw().at<double>(0,2);
112  cam1_intrinsics[3] = rectified?model.right().cy():model.right().K_raw().at<double>(1,2);
113 
114  UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str());
115  UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str());
116  Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
117  UINFO("imuCam=%s", imuCam.prettyPrint().c_str());
118  cv::Mat T_imu_cam0 = imuCam.dataMatrix();
119  cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
120  cv::Vec3d t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
121  R_cam0_imu = R_imu_cam0.t();
122  t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;
123 
124  Transform cam0cam1;
125  if(rectified)
126  {
127  cam0cam1 = Transform(
128  1, 0, 0, -model.baseline(),
129  0, 1, 0, 0,
130  0, 0, 1, 0);
131  }
132  else
133  {
134  cam0cam1 = model.stereoTransform();
135  }
136  UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str());
137  UASSERT(!cam0cam1.isNull());
138  Transform imuCam1 = cam0cam1 * imuCam;
139  cv::Mat T_imu_cam1 = imuCam1.dataMatrix();
140  cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3)));
141  cv::Vec3d t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3));
142  R_cam1_imu = R_imu_cam1.t();
143  t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1;
144  // Processor parameters
145  // get all OdomMSCFK group to make sure all parameters are set
146  ParametersMap parameters = Parameters::getDefaultParameters("OdomMSCKF");
147  uInsert(parameters, parameters_in);
148 
149  Parameters::parse(parameters, Parameters::kOdomMSCKFGridRow(), processor_config.grid_row); //4
150  Parameters::parse(parameters, Parameters::kOdomMSCKFGridCol(), processor_config.grid_col); //4
151  Parameters::parse(parameters, Parameters::kOdomMSCKFGridMinFeatureNum(), processor_config.grid_min_feature_num); //2
152  Parameters::parse(parameters, Parameters::kOdomMSCKFGridMaxFeatureNum(), processor_config.grid_max_feature_num); //4
153  Parameters::parse(parameters, Parameters::kOdomMSCKFPyramidLevels(), processor_config.pyramid_levels); //3
154  Parameters::parse(parameters, Parameters::kOdomMSCKFPatchSize(), processor_config.patch_size); //31
155  Parameters::parse(parameters, Parameters::kOdomMSCKFFastThreshold(), processor_config.fast_threshold); //20
156  Parameters::parse(parameters, Parameters::kOdomMSCKFMaxIteration(), processor_config.max_iteration); //30
157  Parameters::parse(parameters, Parameters::kOdomMSCKFTrackPrecision(), processor_config.track_precision); //0.01
158  Parameters::parse(parameters, Parameters::kOdomMSCKFRansacThreshold(), processor_config.ransac_threshold); //3
159  Parameters::parse(parameters, Parameters::kOdomMSCKFStereoThreshold(), processor_config.stereo_threshold); //3
160 
161  UINFO("===========================================");
162  UINFO("cam0_resolution: %d, %d",
163  cam0_resolution[0], cam0_resolution[1]);
164  UINFO("cam0_intrinscs: %f, %f, %f, %f",
165  cam0_intrinsics[0], cam0_intrinsics[1],
166  cam0_intrinsics[2], cam0_intrinsics[3]);
167  UINFO("cam0_distortion_model: %s",
168  cam0_distortion_model.c_str());
169  UINFO("cam0_distortion_coefficients: %f, %f, %f, %f",
170  cam0_distortion_coeffs[0], cam0_distortion_coeffs[1],
171  cam0_distortion_coeffs[2], cam0_distortion_coeffs[3]);
172 
173  UINFO("cam1_resolution: %d, %d",
174  cam1_resolution[0], cam1_resolution[1]);
175  UINFO("cam1_intrinscs: %f, %f, %f, %f",
176  cam1_intrinsics[0], cam1_intrinsics[1],
177  cam1_intrinsics[2], cam1_intrinsics[3]);
178  UINFO("cam1_distortion_model: %s",
179  cam1_distortion_model.c_str());
180  UINFO("cam1_distortion_coefficients: %f, %f, %f, %f",
181  cam1_distortion_coeffs[0], cam1_distortion_coeffs[1],
182  cam1_distortion_coeffs[2], cam1_distortion_coeffs[3]);
183 
184  std::cout << "R_imu_cam0: " << R_imu_cam0 << std::endl;
185  std::cout << "t_imu_cam0.t(): " << t_imu_cam0.t() << std::endl;
186  std::cout << "R_imu_cam1: " << R_imu_cam1 << std::endl;
187  std::cout << "t_imu_cam1.t(): " << t_imu_cam1.t() << std::endl;
188 
189  UINFO("grid_row: %d",
190  processor_config.grid_row);
191  UINFO("grid_col: %d",
192  processor_config.grid_col);
193  UINFO("grid_min_feature_num: %d",
194  processor_config.grid_min_feature_num);
195  UINFO("grid_max_feature_num: %d",
196  processor_config.grid_max_feature_num);
197  UINFO("pyramid_levels: %d",
198  processor_config.pyramid_levels);
199  UINFO("patch_size: %d",
200  processor_config.patch_size);
201  UINFO("fast_threshold: %d",
202  processor_config.fast_threshold);
203  UINFO("max_iteration: %d",
204  processor_config.max_iteration);
205  UINFO("track_precision: %f",
206  processor_config.track_precision);
207  UINFO("ransac_threshold: %f",
208  processor_config.ransac_threshold);
209  UINFO("stereo_threshold: %f",
210  processor_config.stereo_threshold);
211  UINFO("===========================================");
212 
213  // Create feature detector.
214  detector_ptr = cv::FastFeatureDetector::create(
215  processor_config.fast_threshold);
216  }
217 
218  virtual ~ImageProcessorNoROS() {}
219 
220  msckf_vio::CameraMeasurementPtr stereoCallback2(
221  const sensor_msgs::ImageConstPtr& cam0_img,
222  const sensor_msgs::ImageConstPtr& cam1_img) {
223 
224 
225  //cout << "==================================" << endl;
226 
227  // Get the current image.
228  cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
229  sensor_msgs::image_encodings::MONO8);
230  cam1_curr_img_ptr = cv_bridge::toCvShare(cam1_img,
231  sensor_msgs::image_encodings::MONO8);
232 
233  // Build the image pyramids once since they're used at multiple places
234  createImagePyramids();
235 
236  // Detect features in the first frame.
237  if (is_first_img) {
238  //ros::Time start_time = ros::Time::now();
239  initializeFirstFrame();
240  //UINFO("Detection time: %f",
241  // (ros::Time::now()-start_time).toSec());
242  is_first_img = false;
243 
244  // Draw results.
245  //start_time = ros::Time::now();
246  //drawFeaturesStereo();
247  //UINFO("Draw features: %f",
248  // (ros::Time::now()-start_time).toSec());
249  } else {
250  // Track the feature in the previous image.
251  //ros::Time start_time = ros::Time::now();
252  trackFeatures();
253  //UINFO("Tracking time: %f",
254  // (ros::Time::now()-start_time).toSec());
255 
256  // Add new features into the current image.
257  //start_time = ros::Time::now();
258  addNewFeatures();
259  //UINFO("Addition time: %f",
260  // (ros::Time::now()-start_time).toSec());
261 
262  // Add new features into the current image.
263  //start_time = ros::Time::now();
264  pruneGridFeatures();
265  //UINFO("Prune grid features: %f",
266  // (ros::Time::now()-start_time).toSec());
267 
268  // Draw results.
269  //start_time = ros::Time::now();
270  //drawFeaturesStereo();
271  //UINFO("Draw features: %f",
272  // (ros::Time::now()-start_time).toSec());
273  }
274 
275  //ros::Time start_time = ros::Time::now();
276  //updateFeatureLifetime();
277  //UINFO("Statistics: %f",
278  // (ros::Time::now()-start_time).toSec());
279 
280  // Publish features in the current image.
281  //ros::Time start_time = ros::Time::now();
282  msckf_vio::CameraMeasurementPtr measurements = publish();
283  //UINFO("Publishing: %f",
284  // (ros::Time::now()-start_time).toSec());
285 
286  // Update the previous image and previous features.
287  cam0_prev_img_ptr = cam0_curr_img_ptr;
288  prev_features_ptr = curr_features_ptr;
289  std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
290 
291  // Initialize the current features to empty vectors.
292  curr_features_ptr.reset(new GridFeatures());
293  for (int code = 0; code <
294  processor_config.grid_row*processor_config.grid_col; ++code) {
295  (*curr_features_ptr)[code] = std::vector<FeatureMetaData>(0);
296  }
297 
298  return measurements;
299  }
300 
301  msckf_vio::CameraMeasurementPtr publish() {
302 
303  // Publish features.
304  msckf_vio::CameraMeasurementPtr feature_msg_ptr(new msckf_vio::CameraMeasurement);
305  feature_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
306 
307  std::vector<FeatureIDType> curr_ids(0);
308  std::vector<cv::Point2f> curr_cam0_points(0);
309  std::vector<cv::Point2f> curr_cam1_points(0);
310 
311  for (const auto& grid_features : (*curr_features_ptr)) {
312  for (const auto& feature : grid_features.second) {
313  curr_ids.push_back(feature.id);
314  curr_cam0_points.push_back(feature.cam0_point);
315  curr_cam1_points.push_back(feature.cam1_point);
316  }
317  }
318 
319  std::vector<cv::Point2f> curr_cam0_points_undistorted(0);
320  std::vector<cv::Point2f> curr_cam1_points_undistorted(0);
321 
322  undistortPoints(
323  curr_cam0_points, cam0_intrinsics, cam0_distortion_model,
324  cam0_distortion_coeffs, curr_cam0_points_undistorted);
325  undistortPoints(
326  curr_cam1_points, cam1_intrinsics, cam1_distortion_model,
327  cam1_distortion_coeffs, curr_cam1_points_undistorted);
328 
329  for (unsigned int i = 0; i < curr_ids.size(); ++i) {
330  feature_msg_ptr->features.push_back(msckf_vio::FeatureMeasurement());
331  feature_msg_ptr->features[i].id = curr_ids[i];
332  feature_msg_ptr->features[i].u0 = curr_cam0_points_undistorted[i].x;
333  feature_msg_ptr->features[i].v0 = curr_cam0_points_undistorted[i].y;
334  feature_msg_ptr->features[i].u1 = curr_cam1_points_undistorted[i].x;
335  feature_msg_ptr->features[i].v1 = curr_cam1_points_undistorted[i].y;
336  }
337 
338  //feature_pub.publish(feature_msg_ptr);
339 
340  // Publish tracking info.
341  /*TrackingInfoPtr tracking_info_msg_ptr(new TrackingInfo());
342  tracking_info_msg_ptr->header.stamp = cam0_curr_img_ptr->header.stamp;
343  tracking_info_msg_ptr->before_tracking = before_tracking;
344  tracking_info_msg_ptr->after_tracking = after_tracking;
345  tracking_info_msg_ptr->after_matching = after_matching;
346  tracking_info_msg_ptr->after_ransac = after_ransac;
347  tracking_info_pub.publish(tracking_info_msg_ptr);*/
348 
349  return feature_msg_ptr;
350  }
351 };
352 
353 class MsckfVioNoROS: public msckf_vio::MsckfVio
354 {
355 public:
356  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
357 
358  MsckfVioNoROS(const ParametersMap & parameters_in,
359  const Transform & imuLocalTransform,
360  const StereoCameraModel & model,
361  bool rectified) :
362  msckf_vio::MsckfVio(0)
363  {
364  UDEBUG("");
365  // get all OdomMSCFK group to make sure all parameters are set
366  parameters_ = Parameters::getDefaultParameters("OdomMSCKF");
367  uInsert(parameters_, parameters_in);
368 
369  // Frame id
370  publish_tf = false;
371  frame_rate = 1.0;
372  Parameters::parse(parameters_, Parameters::kOdomMSCKFPositionStdThreshold(), position_std_threshold); //8.0
373 
374  Parameters::parse(parameters_, Parameters::kOdomMSCKFRotationThreshold(), rotation_threshold); //0.2618
375  Parameters::parse(parameters_, Parameters::kOdomMSCKFTranslationThreshold(), translation_threshold); //0.4
376  Parameters::parse(parameters_, Parameters::kOdomMSCKFTrackingRateThreshold(), tracking_rate_threshold); //0.5
377 
378  // Feature optimization parameters
379  Parameters::parse(parameters_, Parameters::kOdomMSCKFOptTranslationThreshold(), msckf_vio::Feature::optimization_config.translation_threshold); //0.2
380 
381  // Noise related parameters
382  Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseGyro(), msckf_vio::IMUState::gyro_noise); //0.001
383  Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseAcc(), msckf_vio::IMUState::acc_noise); //0.01
384  Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseGyroBias(), msckf_vio::IMUState::gyro_bias_noise); //0.001
385  Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseAccBias(), msckf_vio::IMUState::acc_bias_noise); //0.01
386  Parameters::parse(parameters_, Parameters::kOdomMSCKFNoiseFeature(), msckf_vio::Feature::observation_noise); //0.01
387 
388  // Use variance instead of standard deviation.
389  msckf_vio::IMUState::gyro_noise *= msckf_vio::IMUState::gyro_noise;
390  msckf_vio::IMUState::acc_noise *= msckf_vio::IMUState::acc_noise;
391  msckf_vio::IMUState::gyro_bias_noise *= msckf_vio::IMUState::gyro_bias_noise;
392  msckf_vio::IMUState::acc_bias_noise *= msckf_vio::IMUState::acc_bias_noise;
393  msckf_vio::Feature::observation_noise *= msckf_vio::Feature::observation_noise;
394 
395  // Set the initial IMU state.
396  // The intial orientation and position will be set to the origin
397  // implicitly. But the initial velocity and bias can be
398  // set by parameters.
399  // TODO: is it reasonable to set the initial bias to 0?
400  //Parameters::parse(parameters_, "initial_state/velocity/x", state_server.imu_state.velocity(0)); //0.0
401  //Parameters::parse(parameters_, "initial_state/velocity/y", state_server.imu_state.velocity(1)); //0.0
402  //Parameters::parse(parameters_, "initial_state/velocity/z", state_server.imu_state.velocity(2)); //0.0
403 
404  // The initial covariance of orientation and position can be
405  // set to 0. But for velocity, bias and extrinsic parameters,
406  // there should be nontrivial uncertainty.
407  double gyro_bias_cov, acc_bias_cov, velocity_cov;
408  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovVel(), velocity_cov); //0.25
409  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovGyroBias(), gyro_bias_cov); //1e-4
410  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovAccBias(), acc_bias_cov); //1e-2
411 
412  double extrinsic_rotation_cov, extrinsic_translation_cov;
413  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExRot(), extrinsic_rotation_cov); //3.0462e-4
414  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExTrans(), extrinsic_translation_cov); //1e-4
415 
416  state_server.state_cov = Eigen::MatrixXd::Zero(21, 21);
417  for (int i = 3; i < 6; ++i)
418  state_server.state_cov(i, i) = gyro_bias_cov;
419  for (int i = 6; i < 9; ++i)
420  state_server.state_cov(i, i) = velocity_cov;
421  for (int i = 9; i < 12; ++i)
422  state_server.state_cov(i, i) = acc_bias_cov;
423  for (int i = 15; i < 18; ++i)
424  state_server.state_cov(i, i) = extrinsic_rotation_cov;
425  for (int i = 18; i < 21; ++i)
426  state_server.state_cov(i, i) = extrinsic_translation_cov;
427 
428  // Transformation offsets between the frames involved.
429  UINFO("Local transform=%s", model.localTransform().prettyPrint().c_str());
430  UINFO("imuLocalTransform=%s", imuLocalTransform.prettyPrint().c_str());
431  Transform imuCam = model.localTransform().inverse() * imuLocalTransform;
432  UINFO("imuCam=%s", imuCam.prettyPrint().c_str());
433  Eigen::Isometry3d T_imu_cam0(imuCam.toEigen4d());
434  Eigen::Isometry3d T_cam0_imu = T_imu_cam0.inverse();
435 
436  state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
437  state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
438  Transform cam0cam1;
439  if(rectified)
440  {
441  cam0cam1 = Transform(
442  1, 0, 0, -model.baseline(),
443  0, 1, 0, 0,
444  0, 0, 1, 0);
445  }
446  else
447  {
448  cam0cam1 = model.stereoTransform();
449  }
450  UINFO("cam0cam1=%s", cam0cam1.prettyPrint().c_str());
451  msckf_vio::CAMState::T_cam0_cam1 = cam0cam1.toEigen3d().matrix();
452  msckf_vio::IMUState::T_imu_body = imuLocalTransform.toEigen3d().matrix();
453 
454  // Maximum number of camera states to be stored
455  Parameters::parse(parameters_, Parameters::kOdomMSCKFMaxCamStateSize(), max_cam_state_size); //30
456 
457  UINFO("===========================================");
458  UINFO("fixed frame id: %s", fixed_frame_id.c_str());
459  UINFO("child frame id: %s", child_frame_id.c_str());
460  UINFO("publish tf: %d", publish_tf);
461  UINFO("frame rate: %f", frame_rate);
462  UINFO("position std threshold: %f", position_std_threshold);
463  UINFO("Keyframe rotation threshold: %f", rotation_threshold);
464  UINFO("Keyframe translation threshold: %f", translation_threshold);
465  UINFO("Keyframe tracking rate threshold: %f", tracking_rate_threshold);
466  UINFO("gyro noise: %.10f", msckf_vio::IMUState::gyro_noise);
467  UINFO("gyro bias noise: %.10f", msckf_vio::IMUState::gyro_bias_noise);
468  UINFO("acc noise: %.10f", msckf_vio::IMUState::acc_noise);
469  UINFO("acc bias noise: %.10f", msckf_vio::IMUState::acc_bias_noise);
470  UINFO("observation noise: %.10f", msckf_vio::Feature::observation_noise);
471  UINFO("initial velocity: %f, %f, %f",
472  state_server.imu_state.velocity(0),
473  state_server.imu_state.velocity(1),
474  state_server.imu_state.velocity(2));
475  UINFO("initial gyro bias cov: %f", gyro_bias_cov);
476  UINFO("initial acc bias cov: %f", acc_bias_cov);
477  UINFO("initial velocity cov: %f", velocity_cov);
478  UINFO("initial extrinsic rotation cov: %f",
479  extrinsic_rotation_cov);
480  UINFO("initial extrinsic translation cov: %f",
481  extrinsic_translation_cov);
482 
483  std::cout << "T_imu_cam0.linear(): " << T_imu_cam0.linear() << std::endl;
484  std::cout << "T_imu_cam0.translation().transpose(): " << T_imu_cam0.translation().transpose() << std::endl;
485  std::cout << "CAMState::T_cam0_cam1.linear(): " << msckf_vio::CAMState::T_cam0_cam1.linear() << std::endl;
486  std::cout << "CAMState::T_cam0_cam1.translation().transpose(): " << msckf_vio::CAMState::T_cam0_cam1.translation().transpose() << std::endl;
487  std::cout << "IMUState::T_imu_body.linear(): " << msckf_vio::IMUState::T_imu_body.linear() << std::endl;
488  std::cout << "IMUState::T_imu_body.translation().transpose(): " << msckf_vio::IMUState::T_imu_body.translation().transpose() << std::endl;
489 
490  UINFO("max camera state #: %d", max_cam_state_size);
491  UINFO("===========================================");
492 
493  //if (!loadParameters()) return false;
494  //UINFO("Finish loading ROS parameters...");
495 
496  // Initialize state server
497  state_server.continuous_noise_cov =
498  Eigen::Matrix<double, 12, 12>::Zero();
499  state_server.continuous_noise_cov.block<3, 3>(0, 0) =
500  Eigen::Matrix3d::Identity()*msckf_vio::IMUState::gyro_noise;
501  state_server.continuous_noise_cov.block<3, 3>(3, 3) =
502  Eigen::Matrix3d::Identity()*msckf_vio::IMUState::gyro_bias_noise;
503  state_server.continuous_noise_cov.block<3, 3>(6, 6) =
504  Eigen::Matrix3d::Identity()*msckf_vio::IMUState::acc_noise;
505  state_server.continuous_noise_cov.block<3, 3>(9, 9) =
506  Eigen::Matrix3d::Identity()*msckf_vio::IMUState::acc_bias_noise;
507 
508  // Initialize the chi squared test table with confidence
509  // level 0.95.
510  for (int i = 1; i < 100; ++i) {
511  boost::math::chi_squared chi_squared_dist(i);
512  chi_squared_test_table[i] =
513  boost::math::quantile(chi_squared_dist, 0.05);
514  }
515 
516  // if (!createRosIO()) return false;
517  //UINFO("Finish creating ROS IO...");
518  }
519  virtual ~MsckfVioNoROS() {}
520 
521 
522  nav_msgs::Odometry featureCallback2(
523  const msckf_vio::CameraMeasurementConstPtr& msg,
524  pcl::PointCloud<pcl::PointXYZ>::Ptr & localMap) {
525 
526  nav_msgs::Odometry odom;
527 
528  // Return if the gravity vector has not been set.
529  if (!is_gravity_set)
530  {
531  UINFO("Gravity not set yet... waiting for 200 IMU msgs (%d/200)...", (int)imu_msg_buffer.size());
532  return odom;
533  }
534 
535  // Start the system if the first image is received.
536  // The frame where the first image is received will be
537  // the origin.
538  if (is_first_img) {
539  is_first_img = false;
540  state_server.imu_state.time = msg->header.stamp.toSec();
541  }
542 
543  //static double max_processing_time = 0.0;
544  //static int critical_time_cntr = 0;
545  //double processing_start_time = ros::Time::now().toSec();
546 
547  // Propogate the IMU state.
548  // that are received before the image msg.
549  //ros::Time start_time = ros::Time::now();
550  batchImuProcessing(msg->header.stamp.toSec());
551 
552  //double imu_processing_time = (
553  // ros::Time::now()-start_time).toSec();
554 
555  // Augment the state vector.
556  //start_time = ros::Time::now();
557  stateAugmentation(msg->header.stamp.toSec());
558  //double state_augmentation_time = (
559  // ros::Time::now()-start_time).toSec();
560 
561  // Add new observations for existing features or new
562  // features in the map server.
563  //start_time = ros::Time::now();
564  addFeatureObservations(msg);
565  //double add_observations_time = (
566  // ros::Time::now()-start_time).toSec();
567 
568  // Perform measurement update if necessary.
569  //start_time = ros::Time::now();
570  removeLostFeatures();
571  //double remove_lost_features_time = (
572  // ros::Time::now()-start_time).toSec();
573 
574  //start_time = ros::Time::now();
575  pruneCamStateBuffer();
576  //double prune_cam_states_time = (
577  // ros::Time::now()-start_time).toSec();
578 
579  // Publish the odometry.
580  //start_time = ros::Time::now();
581  odom = publish(localMap);
582  //double publish_time = (
583  // ros::Time::now()-start_time).toSec();
584 
585  // Reset the system if necessary.
586  onlineReset2();
587 
588  /*double processing_end_time = ros::Time::now().toSec();
589  double processing_time =
590  processing_end_time - processing_start_time;
591  if (processing_time > 1.0/frame_rate) {
592  ++critical_time_cntr;
593  UINFO("\033[1;31mTotal processing time %f/%d...\033[0m",
594  processing_time, critical_time_cntr);
595  //printf("IMU processing time: %f/%f\n",
596  // imu_processing_time, imu_processing_time/processing_time);
597  //printf("State augmentation time: %f/%f\n",
598  // state_augmentation_time, state_augmentation_time/processing_time);
599  //printf("Add observations time: %f/%f\n",
600  // add_observations_time, add_observations_time/processing_time);
601  printf("Remove lost features time: %f/%f\n",
602  remove_lost_features_time, remove_lost_features_time/processing_time);
603  printf("Remove camera states time: %f/%f\n",
604  prune_cam_states_time, prune_cam_states_time/processing_time);
605  //printf("Publish time: %f/%f\n",
606  // publish_time, publish_time/processing_time);
607  }*/
608 
609  return odom;
610  }
611 
612  void onlineReset2() {
613 
614  // Never perform online reset if position std threshold
615  // is non-positive.
616  if (position_std_threshold <= 0) return;
617  static long long int online_reset_counter = 0;
618 
619  // Check the uncertainty of positions to determine if
620  // the system can be reset.
621  double position_x_std = std::sqrt(state_server.state_cov(12, 12));
622  double position_y_std = std::sqrt(state_server.state_cov(13, 13));
623  double position_z_std = std::sqrt(state_server.state_cov(14, 14));
624 
625  if (position_x_std < position_std_threshold &&
626  position_y_std < position_std_threshold &&
627  position_z_std < position_std_threshold) return;
628 
629  UWARN("Start %lld online reset procedure...",
630  ++online_reset_counter);
631  UINFO("Stardard deviation in xyz: %f, %f, %f",
632  position_x_std, position_y_std, position_z_std);
633 
634  // Remove all existing camera states.
635  state_server.cam_states.clear();
636 
637  // Clear all exsiting features in the map.
638  map_server.clear();
639 
640  // Reset the state covariance.
641  double gyro_bias_cov, acc_bias_cov, velocity_cov;
642  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovVel(), velocity_cov); //0.25
643  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovGyroBias(), gyro_bias_cov); //1e-4
644  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovAccBias(), acc_bias_cov); //1e-2
645 
646  double extrinsic_rotation_cov, extrinsic_translation_cov;
647  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExRot(), extrinsic_rotation_cov); //3.0462e-4
648  Parameters::parse(parameters_, Parameters::kOdomMSCKFInitCovExTrans(), extrinsic_translation_cov); //1e-4
649 
650 
651  state_server.state_cov = Eigen::MatrixXd::Zero(21, 21);
652  for (int i = 3; i < 6; ++i)
653  state_server.state_cov(i, i) = gyro_bias_cov;
654  for (int i = 6; i < 9; ++i)
655  state_server.state_cov(i, i) = velocity_cov;
656  for (int i = 9; i < 12; ++i)
657  state_server.state_cov(i, i) = acc_bias_cov;
658  for (int i = 15; i < 18; ++i)
659  state_server.state_cov(i, i) = extrinsic_rotation_cov;
660  for (int i = 18; i < 21; ++i)
661  state_server.state_cov(i, i) = extrinsic_translation_cov;
662 
663  UWARN("%lld online reset complete...", online_reset_counter);
664  return;
665  }
666 
667  nav_msgs::Odometry publish(pcl::PointCloud<pcl::PointXYZ>::Ptr & feature_msg_ptr) {
668 
669  // Convert the IMU frame to the body frame.
670  const msckf_vio::IMUState& imu_state = state_server.imu_state;
671  Eigen::Isometry3d T_i_w = Eigen::Isometry3d::Identity();
672  T_i_w.linear() = msckf_vio::quaternionToRotation(imu_state.orientation).transpose();
673  T_i_w.translation() = imu_state.position;
674 
675  Eigen::Isometry3d T_b_w = T_i_w * msckf_vio::IMUState::T_imu_body.inverse();
676  Eigen::Vector3d body_velocity =
677  msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
678 
679  // Publish tf
680  /*if (publish_tf) {
681  tf::Transform T_b_w_tf;
682  tf::transformEigenToTF(T_b_w, T_b_w_tf);
683  tf_pub.sendTransform(tf::StampedTransform(
684  T_b_w_tf, time, fixed_frame_id, child_frame_id));
685  }*/
686 
687  // Publish the odometry
688  nav_msgs::Odometry odom_msg;
689  //odom_msg.header.stamp = time;
690  odom_msg.header.frame_id = fixed_frame_id;
691  odom_msg.child_frame_id = child_frame_id;
692 
693  tf::poseEigenToMsg(T_b_w, odom_msg.pose.pose);
694  tf::vectorEigenToMsg(body_velocity, odom_msg.twist.twist.linear);
695 
696  // Convert the covariance.
697  Eigen::Matrix3d P_oo = state_server.state_cov.block<3, 3>(0, 0);
698  Eigen::Matrix3d P_op = state_server.state_cov.block<3, 3>(0, 12);
699  Eigen::Matrix3d P_po = state_server.state_cov.block<3, 3>(12, 0);
700  Eigen::Matrix3d P_pp = state_server.state_cov.block<3, 3>(12, 12);
701  Eigen::Matrix<double, 6, 6> P_imu_pose = Eigen::Matrix<double, 6, 6>::Zero();
702  P_imu_pose << P_pp, P_po, P_op, P_oo;
703 
704  Eigen::Matrix<double, 6, 6> H_pose = Eigen::Matrix<double, 6, 6>::Zero();
705  H_pose.block<3, 3>(0, 0) = msckf_vio::IMUState::T_imu_body.linear();
706  H_pose.block<3, 3>(3, 3) = msckf_vio::IMUState::T_imu_body.linear();
707  Eigen::Matrix<double, 6, 6> P_body_pose = H_pose *
708  P_imu_pose * H_pose.transpose();
709 
710  for (int i = 0; i < 6; ++i)
711  for (int j = 0; j < 6; ++j)
712  odom_msg.pose.covariance[6*i+j] = P_body_pose(i, j);
713 
714  // Construct the covariance for the velocity.
715  Eigen::Matrix3d P_imu_vel = state_server.state_cov.block<3, 3>(6, 6);
716  Eigen::Matrix3d H_vel = msckf_vio::IMUState::T_imu_body.linear();
717  Eigen::Matrix3d P_body_vel = H_vel * P_imu_vel * H_vel.transpose();
718  for (int i = 0; i < 3; ++i)
719  for (int j = 0; j < 3; ++j)
720  odom_msg.twist.covariance[i*6+j] = P_body_vel(i, j);
721 
722  // odom_pub.publish(odom_msg);
723 
724  // Publish the 3D positions of the features that
725  // has been initialized.
726  feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
727  feature_msg_ptr->header.frame_id = fixed_frame_id;
728  feature_msg_ptr->height = 1;
729  for (const auto& item : map_server) {
730  const auto& feature = item.second;
731  if (feature.is_initialized) {
732  feature_msg_ptr->points.push_back(pcl::PointXYZ(
733  feature.position(0), feature.position(1), feature.position(2)));
734  }
735  }
736  feature_msg_ptr->width = feature_msg_ptr->points.size();
737 
738  // feature_pub.publish(feature_msg_ptr);
739 
740  return odom_msg;
741  }
742 
743 private:
744  ParametersMap parameters_;
745 };
746 #endif
747 
748 OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
749  Odometry(parameters)
750 #ifdef RTABMAP_MSCKF_VIO
751 ,
752 imageProcessor_(0),
753 msckf_(0),
754 parameters_(parameters),
755 fixPoseRotation_(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0),
756 previousPose_(Transform::getIdentity()),
757 initGravity_(false)
758 #endif
759 {
760 }
761 
762 OdometryMSCKF::~OdometryMSCKF()
763 {
764  UDEBUG("");
765 #ifdef RTABMAP_MSCKF_VIO
766  delete imageProcessor_;
767  delete msckf_;
768 #endif
769 }
770 
771 void OdometryMSCKF::reset(const Transform & initialPose)
772 {
773  Odometry::reset(initialPose);
774 #ifdef RTABMAP_MSCKF_VIO
775  if(!initGravity_)
776  {
777  if(imageProcessor_)
778  {
779  delete imageProcessor_;
780  imageProcessor_ = 0;
781  }
782  if(msckf_)
783  {
784  delete msckf_;
785  msckf_ = 0;
786  }
787  lastImu_ = IMU();
788  previousPose_.setIdentity();
789  }
790  initGravity_ = false;
791 #endif
792 }
793 
794 // return not null transform if odometry is correctly computed
795 Transform OdometryMSCKF::computeTransform(
796  SensorData & data,
797  const Transform & guess,
798  OdometryInfo * info)
799 {
800  UDEBUG("");
801  Transform t;
802 
803 #ifdef RTABMAP_MSCKF_VIO
804  UTimer timer;
805 
806  if(!data.imu().empty())
807  {
808  UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
809  data.imu().linearAcceleration()[0],
810  data.imu().linearAcceleration()[1],
811  data.imu().linearAcceleration()[2],
812  data.imu().angularVelocity()[0],
813  data.imu().angularVelocity()[1],
814  data.imu().angularVelocity()[2]);
815  if(imageProcessor_ && msckf_)
816  {
817  sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
818  msg->angular_velocity.x = data.imu().angularVelocity()[0];
819  msg->angular_velocity.y = data.imu().angularVelocity()[1];
820  msg->angular_velocity.z = data.imu().angularVelocity()[2];
821  msg->linear_acceleration.x = data.imu().linearAcceleration()[0];
822  msg->linear_acceleration.y = data.imu().linearAcceleration()[1];
823  msg->linear_acceleration.z = data.imu().linearAcceleration()[2];
824  msg->header.stamp.fromSec(data.stamp());
825  imageProcessor_->imuCallback(msg);
826  msckf_->imuCallback(msg);
827  }
828  else
829  {
830  UWARN("Ignoring IMU, waiting for an image to initialize...");
831  lastImu_ = data.imu();
832  }
833  }
834 
835  if(!data.imageRaw().empty() && !data.rightRaw().empty())
836  {
837  UDEBUG("Image update stamp=%f", data.stamp());
838  if(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
839  {
840  if(msckf_ == 0)
841  {
842  UINFO("Initialization");
843  if(lastImu_.empty())
844  {
845  UWARN("Ignoring Image, waiting for imu to initialize...");
846  return t;
847  }
848  UINFO("Creating ImageProcessorNoROS...");
849  imageProcessor_ = new ImageProcessorNoROS(
850  parameters_,
851  lastImu_.localTransform(),
852  data.stereoCameraModels()[0],
853  this->imagesAlreadyRectified());
854  UINFO("Creating MsckfVioNoROS...");
855  msckf_ = new MsckfVioNoROS(
856  parameters_,
857  lastImu_.localTransform(),
858  data.stereoCameraModels()[0],
859  this->imagesAlreadyRectified());
860  }
861 
862  // Convert to ROS
863  cv_bridge::CvImage cam0;
864  cv_bridge::CvImage cam1;
865  cam0.header.stamp.fromSec(data.stamp());
866  cam1.header.stamp.fromSec(data.stamp());
867 
868  if(data.imageRaw().type() == CV_8UC3)
869  {
870  cv::cvtColor(data.imageRaw(), cam0.image, CV_BGR2GRAY);
871  }
872  else
873  {
874  cam0.image = data.imageRaw();
875  }
876  if(data.rightRaw().type() == CV_8UC3)
877  {
878  cv::cvtColor(data.rightRaw(), cam1.image, CV_BGR2GRAY);
879  }
880  else
881  {
882  cam1.image = data.rightRaw();
883  }
884 
885  sensor_msgs::ImagePtr cam0Msg(new sensor_msgs::Image);
886  sensor_msgs::ImagePtr cam1Msg(new sensor_msgs::Image);
887  cam0.toImageMsg(*cam0Msg);
888  cam1.toImageMsg(*cam1Msg);
889  cam0Msg->encoding = sensor_msgs::image_encodings::MONO8;
890  cam1Msg->encoding = sensor_msgs::image_encodings::MONO8;
891 
892  msckf_vio::CameraMeasurementPtr measurements = imageProcessor_->stereoCallback2(cam0Msg, cam1Msg);
893  pcl::PointCloud<pcl::PointXYZ>::Ptr localMap;
894  nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap);
895 
896  if( odom.pose.pose.orientation.x != 0.0f ||
897  odom.pose.pose.orientation.y != 0.0f ||
898  odom.pose.pose.orientation.z != 0.0f ||
899  odom.pose.pose.orientation.w != 0.0f)
900  {
901  Transform p = Transform(
902  odom.pose.pose.position.x,
903  odom.pose.pose.position.y,
904  odom.pose.pose.position.z,
905  odom.pose.pose.orientation.x,
906  odom.pose.pose.orientation.y,
907  odom.pose.pose.orientation.z,
908  odom.pose.pose.orientation.w);
909 
910  p = fixPoseRotation_*p;
911 
912  if(this->getPose().rotation().isIdentity())
913  {
914  initGravity_ = true;
915  this->reset(this->getPose()*p.rotation());
916  }
917 
918  if(previousPose_.isIdentity())
919  {
920  previousPose_ = p;
921  }
922 
923  // make it incremental
924  Transform previousPoseInv = previousPose_.inverse();
925  t = previousPoseInv*p;
926  previousPose_ = p;
927 
928  if(info)
929  {
930  info->type = this->getType();
931  info->features = measurements->features.size();
932 
933  info->reg.covariance = cv::Mat::zeros(6, 6, CV_64FC1);
934  cv::Mat twistCov(6,6,CV_64FC1, odom.twist.covariance.elems);
935  // twist covariance is not in base frame, but in world frame,
936  // we have to convert the covariance in base frame
937  cv::Matx31f covWorldFrame(twistCov.at<double>(0, 0),
938  twistCov.at<double>(1, 1),
939  twistCov.at<double>(2, 2));
940  cv::Matx31f covBaseFrame = cv::Matx33f(previousPoseInv.rotationMatrix()) * covWorldFrame;
941  // we set only diagonal values as there is an issue with g2o and off-diagonal values
942  info->reg.covariance.at<double>(0, 0) = fabs(covBaseFrame.val[0])/10.0;
943  info->reg.covariance.at<double>(1, 1) = fabs(covBaseFrame.val[1])/10.0;
944  info->reg.covariance.at<double>(2, 2) = fabs(covBaseFrame.val[2])/10.0;
945  if(info->reg.covariance.at<double>(0, 0) < 0.0001)
946  {
947  info->reg.covariance.at<double>(0, 0) = 0.0001;
948  }
949  if(info->reg.covariance.at<double>(1, 1) < 0.0001)
950  {
951  info->reg.covariance.at<double>(1, 1) = 0.0001;
952  }
953  if(info->reg.covariance.at<double>(2, 2) < 0.0001)
954  {
955  info->reg.covariance.at<double>(2, 2) = 0.0001;
956  }
957  info->reg.covariance.at<double>(3, 3) = msckf_vio::IMUState::gyro_noise*10.0;
958  info->reg.covariance.at<double>(4, 4) = info->reg.covariance.at<double>(3, 3);
959  info->reg.covariance.at<double>(5, 5) = info->reg.covariance.at<double>(3, 3);
960 
961  if(this->isInfoDataFilled())
962  {
963  if(localMap.get() && localMap->size())
964  {
965  Eigen::Affine3f fixRot = fixPoseRotation_.toEigen3f();
966  for(unsigned int i=0; i<localMap->size(); ++i)
967  {
968  pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), fixRot);
969  info->localMap.insert(std::make_pair(i, cv::Point3f(pt.x, pt.y, pt.z)));
970  }
971  }
972  if(this->imagesAlreadyRectified())
973  {
974  info->newCorners.resize(measurements->features.size());
975  float fx = data.stereoCameraModels()[0].left().fx();
976  float fy = data.stereoCameraModels()[0].left().fy();
977  float cx = data.stereoCameraModels()[0].left().cx();
978  float cy = data.stereoCameraModels()[0].left().cy();
979  info->reg.inliersIDs.resize(measurements->features.size());
980  for(unsigned int i=0; i<measurements->features.size(); ++i)
981  {
982  info->newCorners[i].x = measurements->features[i].u0*fx+cx;
983  info->newCorners[i].y = measurements->features[i].v0*fy+cy;
984  info->reg.inliersIDs[i] = i;
985  }
986  }
987  }
988  }
989  UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
990  }
991  }
992  }
993 
994 #else
995  UERROR("RTAB-Map is not built with MSCKF_VIO support! Select another visual odometry approach.");
996 #endif
997  return t;
998 }
999 
1000 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::OdometryMSCKF::~OdometryMSCKF
virtual ~OdometryMSCKF()
Definition: OdometryMSCKF.cpp:762
UINFO
#define UINFO(...)
rtabmap::OdometryMSCKF::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryMSCKF.cpp:795
cy
const double cy
OdometryInfo.h
Eigen::Transform
timer
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
OdometryMSCKF.h
fx
const double fx
this
this
rtabmap::OdometryInfo::features
int features
Definition: OdometryInfo.h:100
rtabmap::OdometryMSCKF::OdometryMSCKF
OdometryMSCKF(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryMSCKF.cpp:748
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
fabs
Real fabs(const Real &a)
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::IMU
Definition: IMU.h:19
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:896
rtabmap::OdometryMSCKF::getType
virtual Odometry::Type getType()
Definition: OdometryMSCKF.h:45
data
int data[]
util3d_transforms.h
j
std::ptrdiff_t j
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
cv_bridge::CvImage::header
std_msgs::Header header
std::swap
void swap(GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &a, GeographicLib::NearestNeighbor< dist_t, pos_t, distfun_t > &b)
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
info
else if n * info
rtabmap::Transform::rotationMatrix
cv::Mat rotationMatrix() const
Definition: Transform.cpp:248
rtabmap::OdometryMSCKF::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryMSCKF.cpp:771
UASSERT
#define UASSERT(condition)
p
Point3_ p(2)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Eigen::Transform::inverse
EIGEN_DEVICE_FUNC Transform inverse(TransformTraits traits=(TransformTraits) Mode) const
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
cam1
Camera cam1(interp_pose1, emptyK)
UWARN
#define UWARN(...)
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
timer::elapsed
double elapsed() const
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
Eigen::Transform::linear
EIGEN_DEVICE_FUNC LinearPart linear()
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
sensor_msgs::image_encodings::MONO8
const std::string MONO8
UStl.h
Wrappers of STL for convenient functions.
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:134
UThread.h
cv_bridge::CvImage
UDEBUG
#define UDEBUG(...)
Eigen::Transform::Identity
static const EIGEN_DEVICE_FUNC Transform Identity()
rtabmap::Odometry
Definition: Odometry.h:42
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
Eigen::Matrix
false
#define false
Definition: ConvertUTF.c:56
cx
const double cx
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
Eigen::Transform::translation
EIGEN_DEVICE_FUNC TranslationPart translation()
t
Point2 t(10, 10)
rtabmap::Odometry::imagesAlreadyRectified
bool imagesAlreadyRectified() const
Definition: Odometry.h:83
rtabmap
Definition: CameraARCore.cpp:35
code
code
UERROR
#define UERROR(...)
cv_bridge::CvImage::image
cv::Mat image
trace.model
model
Definition: trace.py:4
i
int i
fy
const double fy
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77
msg
msg

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