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 = msckf_vio::IMUState::T_imu_body * T_i_w *
676  msckf_vio::IMUState::T_imu_body.inverse();
677  Eigen::Vector3d body_velocity =
678  msckf_vio::IMUState::T_imu_body.linear() * imu_state.velocity;
679 
680  // Publish tf
681  /*if (publish_tf) {
682  tf::Transform T_b_w_tf;
683  tf::transformEigenToTF(T_b_w, T_b_w_tf);
684  tf_pub.sendTransform(tf::StampedTransform(
685  T_b_w_tf, time, fixed_frame_id, child_frame_id));
686  }*/
687 
688  // Publish the odometry
689  nav_msgs::Odometry odom_msg;
690  //odom_msg.header.stamp = time;
691  odom_msg.header.frame_id = fixed_frame_id;
692  odom_msg.child_frame_id = child_frame_id;
693 
694  tf::poseEigenToMsg(T_b_w, odom_msg.pose.pose);
695  tf::vectorEigenToMsg(body_velocity, odom_msg.twist.twist.linear);
696 
697  // Convert the covariance.
698  Eigen::Matrix3d P_oo = state_server.state_cov.block<3, 3>(0, 0);
699  Eigen::Matrix3d P_op = state_server.state_cov.block<3, 3>(0, 12);
700  Eigen::Matrix3d P_po = state_server.state_cov.block<3, 3>(12, 0);
701  Eigen::Matrix3d P_pp = state_server.state_cov.block<3, 3>(12, 12);
702  Eigen::Matrix<double, 6, 6> P_imu_pose = Eigen::Matrix<double, 6, 6>::Zero();
703  P_imu_pose << P_pp, P_po, P_op, P_oo;
704 
705  Eigen::Matrix<double, 6, 6> H_pose = Eigen::Matrix<double, 6, 6>::Zero();
706  H_pose.block<3, 3>(0, 0) = msckf_vio::IMUState::T_imu_body.linear();
707  H_pose.block<3, 3>(3, 3) = msckf_vio::IMUState::T_imu_body.linear();
708  Eigen::Matrix<double, 6, 6> P_body_pose = H_pose *
709  P_imu_pose * H_pose.transpose();
710 
711  for (int i = 0; i < 6; ++i)
712  for (int j = 0; j < 6; ++j)
713  odom_msg.pose.covariance[6*i+j] = P_body_pose(i, j);
714 
715  // Construct the covariance for the velocity.
716  Eigen::Matrix3d P_imu_vel = state_server.state_cov.block<3, 3>(6, 6);
717  Eigen::Matrix3d H_vel = msckf_vio::IMUState::T_imu_body.linear();
718  Eigen::Matrix3d P_body_vel = H_vel * P_imu_vel * H_vel.transpose();
719  for (int i = 0; i < 3; ++i)
720  for (int j = 0; j < 3; ++j)
721  odom_msg.twist.covariance[i*6+j] = P_body_vel(i, j);
722 
723  // odom_pub.publish(odom_msg);
724 
725  // Publish the 3D positions of the features that
726  // has been initialized.
727  feature_msg_ptr.reset(new pcl::PointCloud<pcl::PointXYZ>());
728  feature_msg_ptr->header.frame_id = fixed_frame_id;
729  feature_msg_ptr->height = 1;
730  for (const auto& item : map_server) {
731  const auto& feature = item.second;
732  if (feature.is_initialized) {
733  Eigen::Vector3d feature_position =
734  msckf_vio::IMUState::T_imu_body.linear() * feature.position;
735  feature_msg_ptr->points.push_back(pcl::PointXYZ(
736  feature_position(0), feature_position(1), feature_position(2)));
737  }
738  }
739  feature_msg_ptr->width = feature_msg_ptr->points.size();
740 
741  //feature_pub.publish(feature_msg_ptr);
742 
743  return odom_msg;
744  }
745 
746 private:
747  ParametersMap parameters_;
748 };
749 #endif
750 
751 OdometryMSCKF::OdometryMSCKF(const ParametersMap & parameters) :
752  Odometry(parameters)
753 #ifdef RTABMAP_MSCKF_VIO
754 ,
755 imageProcessor_(0),
756 msckf_(0),
757 parameters_(parameters),
758 fixPoseRotation_(0, 0, -1, 0, 0, 1, 0, 0, 1, 0, 0, 0),
759 previousPose_(Transform::getIdentity()),
760 initGravity_(false)
761 #endif
762 {
763 }
764 
765 OdometryMSCKF::~OdometryMSCKF()
766 {
767  UDEBUG("");
768 #ifdef RTABMAP_MSCKF_VIO
769  delete imageProcessor_;
770  delete msckf_;
771 #endif
772 }
773 
774 void OdometryMSCKF::reset(const Transform & initialPose)
775 {
776  Odometry::reset(initialPose);
777 #ifdef RTABMAP_MSCKF_VIO
778  if(!initGravity_)
779  {
780  if(imageProcessor_)
781  {
782  delete imageProcessor_;
783  imageProcessor_ = 0;
784  }
785  if(msckf_)
786  {
787  delete msckf_;
788  msckf_ = 0;
789  }
790  lastImu_ = IMU();
791  previousPose_.setIdentity();
792  }
793  initGravity_ = false;
794 #endif
795 }
796 
797 // return not null transform if odometry is correctly computed
798 Transform OdometryMSCKF::computeTransform(
799  SensorData & data,
800  const Transform & guess,
801  OdometryInfo * info)
802 {
803  UDEBUG("");
804  Transform t;
805 
806 #ifdef RTABMAP_MSCKF_VIO
807  UTimer timer;
808 
809  if(!data.imu().empty())
810  {
811  UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
812  data.imu().linearAcceleration()[0],
813  data.imu().linearAcceleration()[1],
814  data.imu().linearAcceleration()[2],
815  data.imu().angularVelocity()[0],
816  data.imu().angularVelocity()[1],
817  data.imu().angularVelocity()[2]);
818  if(imageProcessor_ && msckf_)
819  {
820  sensor_msgs::ImuPtr msg(new sensor_msgs::Imu);
821  msg->angular_velocity.x = data.imu().angularVelocity()[0];
822  msg->angular_velocity.y = data.imu().angularVelocity()[1];
823  msg->angular_velocity.z = data.imu().angularVelocity()[2];
824  msg->linear_acceleration.x = data.imu().linearAcceleration()[0];
825  msg->linear_acceleration.y = data.imu().linearAcceleration()[1];
826  msg->linear_acceleration.z = data.imu().linearAcceleration()[2];
827  msg->header.stamp.fromSec(data.stamp());
828  imageProcessor_->imuCallback(msg);
829  msckf_->imuCallback(msg);
830  }
831  else
832  {
833  UWARN("Ignoring IMU, waiting for an image to initialize...");
834  lastImu_ = data.imu();
835  }
836  }
837 
838  if(!data.imageRaw().empty() && !data.rightRaw().empty())
839  {
840  UDEBUG("Image update stamp=%f", data.stamp());
841  if(data.stereoCameraModel().isValidForProjection())
842  {
843  if(msckf_ == 0)
844  {
845  UINFO("Initialization");
846  if(lastImu_.empty())
847  {
848  UWARN("Ignoring Image, waiting for imu to initialize...");
849  return t;
850  }
851  UINFO("Creating ImageProcessorNoROS...");
852  imageProcessor_ = new ImageProcessorNoROS(
853  parameters_,
854  lastImu_.localTransform(),
855  data.stereoCameraModel(),
856  this->imagesAlreadyRectified());
857  UINFO("Creating MsckfVioNoROS...");
858  msckf_ = new MsckfVioNoROS(
859  parameters_,
860  lastImu_.localTransform(),
861  data.stereoCameraModel(),
862  this->imagesAlreadyRectified());
863  }
864 
865  // Convert to ROS
866  cv_bridge::CvImage cam0;
867  cv_bridge::CvImage cam1;
868  cam0.header.stamp.fromSec(data.stamp());
869  cam1.header.stamp.fromSec(data.stamp());
870 
871  if(data.imageRaw().type() == CV_8UC3)
872  {
873  cv::cvtColor(data.imageRaw(), cam0.image, CV_BGR2GRAY);
874  }
875  else
876  {
877  cam0.image = data.imageRaw();
878  }
879  if(data.rightRaw().type() == CV_8UC3)
880  {
881  cv::cvtColor(data.rightRaw(), cam1.image, CV_BGR2GRAY);
882  }
883  else
884  {
885  cam1.image = data.rightRaw();
886  }
887 
888  sensor_msgs::ImagePtr cam0Msg(new sensor_msgs::Image);
889  sensor_msgs::ImagePtr cam1Msg(new sensor_msgs::Image);
890  cam0.toImageMsg(*cam0Msg);
891  cam1.toImageMsg(*cam1Msg);
892  cam0Msg->encoding = sensor_msgs::image_encodings::MONO8;
893  cam1Msg->encoding = sensor_msgs::image_encodings::MONO8;
894 
895  msckf_vio::CameraMeasurementPtr measurements = imageProcessor_->stereoCallback2(cam0Msg, cam1Msg);
896  pcl::PointCloud<pcl::PointXYZ>::Ptr localMap;
897  nav_msgs::Odometry odom = msckf_->featureCallback2(measurements, localMap);
898 
899  if( odom.pose.pose.orientation.x != 0.0f ||
900  odom.pose.pose.orientation.y != 0.0f ||
901  odom.pose.pose.orientation.z != 0.0f ||
902  odom.pose.pose.orientation.w != 0.0f)
903  {
904  Transform p = Transform(
905  odom.pose.pose.position.x,
906  odom.pose.pose.position.y,
907  odom.pose.pose.position.z,
908  odom.pose.pose.orientation.x,
909  odom.pose.pose.orientation.y,
910  odom.pose.pose.orientation.z,
911  odom.pose.pose.orientation.w);
912 
913  p = fixPoseRotation_*p;
914 
915  if(this->getPose().rotation().isIdentity())
916  {
917  initGravity_ = true;
918  this->reset(this->getPose()*p.rotation());
919  }
920 
921  if(previousPose_.isIdentity())
922  {
923  previousPose_ = p;
924  }
925 
926  // make it incremental
927  Transform previousPoseInv = previousPose_.inverse();
928  t = previousPoseInv*p;
929  previousPose_ = p;
930 
931  if(info)
932  {
933  info->type = this->getType();
934  info->features = measurements->features.size();
935 
936  info->reg.covariance = cv::Mat::zeros(6, 6, CV_64FC1);
937  cv::Mat twistCov(6,6,CV_64FC1, odom.twist.covariance.elems);
938  // twist covariance is not in base frame, but in world frame,
939  // we have to convert the covariance in base frame
940  cv::Matx31f covWorldFrame(twistCov.at<double>(0, 0),
941  twistCov.at<double>(1, 1),
942  twistCov.at<double>(2, 2));
943  cv::Matx31f covBaseFrame = cv::Matx33f(previousPoseInv.rotationMatrix()) * covWorldFrame;
944  // we set only diagonal values as there is an issue with g2o and off-diagonal values
945  info->reg.covariance.at<double>(0, 0) = fabs(covBaseFrame.val[0])/10.0;
946  info->reg.covariance.at<double>(1, 1) = fabs(covBaseFrame.val[1])/10.0;
947  info->reg.covariance.at<double>(2, 2) = fabs(covBaseFrame.val[2])/10.0;
948  if(info->reg.covariance.at<double>(0, 0) < 0.0001)
949  {
950  info->reg.covariance.at<double>(0, 0) = 0.0001;
951  }
952  if(info->reg.covariance.at<double>(1, 1) < 0.0001)
953  {
954  info->reg.covariance.at<double>(1, 1) = 0.0001;
955  }
956  if(info->reg.covariance.at<double>(2, 2) < 0.0001)
957  {
958  info->reg.covariance.at<double>(2, 2) = 0.0001;
959  }
960  info->reg.covariance.at<double>(3, 3) = msckf_vio::IMUState::gyro_noise*10.0;
961  info->reg.covariance.at<double>(4, 4) = info->reg.covariance.at<double>(3, 3);
962  info->reg.covariance.at<double>(5, 5) = info->reg.covariance.at<double>(3, 3);
963 
964  if(this->isInfoDataFilled())
965  {
966  if(localMap.get() && localMap->size())
967  {
968  Eigen::Affine3f fixRot = fixPoseRotation_.toEigen3f();
969  for(unsigned int i=0; i<localMap->size(); ++i)
970  {
971  pcl::PointXYZ pt = pcl::transformPoint(localMap->at(i), fixRot);
972  info->localMap.insert(std::make_pair(i, cv::Point3f(pt.x, pt.y, pt.z)));
973  }
974  }
975  if(this->imagesAlreadyRectified())
976  {
977  info->newCorners.resize(measurements->features.size());
978  float fx = data.stereoCameraModel().left().fx();
979  float fy = data.stereoCameraModel().left().fy();
980  float cx = data.stereoCameraModel().left().cx();
981  float cy = data.stereoCameraModel().left().cy();
982  info->reg.inliersIDs.resize(measurements->features.size());
983  for(unsigned int i=0; i<measurements->features.size(); ++i)
984  {
985  info->newCorners[i].x = measurements->features[i].u0*fx+cx;
986  info->newCorners[i].y = measurements->features[i].v0*fy+cy;
987  info->reg.inliersIDs[i] = i;
988  }
989  }
990  }
991  }
992  UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
993  }
994  }
995  }
996 
997 #else
998  UERROR("RTAB-Map is not built with MSCKF_VIO support! Select another visual odometry approach.");
999 #endif
1000  return t;
1001 }
1002 
1003 } // namespace rtabmap
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:476
UTimer
Definition: UTimer.h:46
rtabmap::OdometryMSCKF::OdometryMSCKF
OdometryMSCKF(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryMSCKF.cpp:751
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:63
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:74
rtabmap::Odometry::imagesAlreadyRectified
bool imagesAlreadyRectified() const
Definition: Odometry.h:79
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:191
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Transform::rotationMatrix
cv::Mat rotationMatrix() const
Definition: Transform.cpp:217
rtabmap::OdometryInfo::localMap
std::map< int, cv::Point3f > localMap
Definition: OdometryInfo.h:124
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:73
rtabmap::OdometryMSCKF::getType
virtual Odometry::Type getType()
Definition: OdometryMSCKF.h:45
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:81
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:190
UASSERT
#define UASSERT(condition)
UStl.h
Wrappers of STL for convenient functions.
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:122
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
rtabmap::IMU::linearAcceleration
const cv::Vec3d linearAcceleration() const
Definition: IMU.h:58
cv_bridge::CvImage
rtabmap::OdometryMSCKF::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryMSCKF.cpp:798
sensor_msgs::image_encodings::MONO8
const std::string MONO8
util3d_transforms.h
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:157
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
glm::isIdentity
GLM_FUNC_DECL bool isIdentity(matType< T, P > const &m, T const &epsilon)
cv_bridge::CvImage::image
cv::Mat image
false
#define false
Definition: ConvertUTF.c:56
rtabmap::SensorData::imu
const IMU & imu() const
Definition: SensorData.h:269
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:778
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
UERROR
#define UERROR(...)
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
UWARN
#define UWARN(...)
rtabmap::OdometryInfo::newCorners
std::vector< cv::Point2f > newCorners
Definition: OdometryInfo.h:129
rtabmap::OdometryInfo::reg
RegistrationInfo reg
Definition: OdometryInfo.h:96
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::OdometryMSCKF::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryMSCKF.cpp:774
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
cv_bridge::CvImage::header
std_msgs::Header header
rtabmap::IMU::angularVelocity
const cv::Vec3d & angularVelocity() const
Definition: IMU.h:55
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:59