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