Skip to content

Instantly share code, notes, and snippets.

@borongyuan

borongyuan/msckf_vio_68065ad_ros_commented.patch

Last active September 15, 2023 15:51
Show Gist options
  • Save borongyuan/90f10aa6969ebccdba2b19ce0cfa77ea to your computer and use it in GitHub Desktop.
Save borongyuan/90f10aa6969ebccdba2b19ce0cfa77ea to your computer and use it in GitHub Desktop.
Download ZIP
MSCKF VIO patch to disable ROS stuff to be compiled inside RTAB-Map
Raw
msckf_vio_68065ad_ros_commented.patch
diff --git a/include/msckf_vio/image_processor.h b/include/msckf_vio/image_processor.h
index 086c5a9..640a243 100644
--- a/include/msckf_vio/image_processor.h
+++ b/include/msckf_vio/image_processor.h
@@ -31,7 +31,7 @@ namespace msckf_vio {
class ImageProcessor {
public:
// Constructor
- ImageProcessor(ros::NodeHandle& n);
+ ImageProcessor(ros::NodeHandle* n);
// Disable copy and assign constructors.
ImageProcessor(const ImageProcessor&) = delete;
ImageProcessor operator=(const ImageProcessor&) = delete;
@@ -45,7 +45,7 @@ public:
typedef boost::shared_ptr<ImageProcessor> Ptr;
typedef boost::shared_ptr<const ImageProcessor> ConstPtr;
-private:
+public:
/*
* @brief ProcessorConfig Configuration parameters for
@@ -370,10 +370,10 @@ private:
int after_ransac;
// Ros node handle
- ros::NodeHandle nh;
+ ros::NodeHandle * nh;
// Subscribers and publishers.
- message_filters::Subscriber<
+ /*message_filters::Subscriber<
sensor_msgs::Image> cam0_img_sub;
message_filters::Subscriber<
sensor_msgs::Image> cam1_img_sub;
@@ -382,7 +382,7 @@ private:
ros::Subscriber imu_sub;
ros::Publisher feature_pub;
ros::Publisher tracking_info_pub;
- image_transport::Publisher debug_stereo_pub;
+ image_transport::Publisher debug_stereo_pub;*/
// Debugging
std::map<FeatureIDType, int> feature_lifetime;
diff --git a/include/msckf_vio/msckf_vio.h b/include/msckf_vio/msckf_vio.h
index 8077acf..98f7cf7 100644
--- a/include/msckf_vio/msckf_vio.h
+++ b/include/msckf_vio/msckf_vio.h
@@ -40,7 +40,7 @@ class MsckfVio {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
// Constructor
- MsckfVio(ros::NodeHandle& pnh);
+ MsckfVio(ros::NodeHandle* pnh);
// Disable copy and assign constructor
MsckfVio(const MsckfVio&) = delete;
MsckfVio operator=(const MsckfVio&) = delete;
@@ -61,7 +61,7 @@ class MsckfVio {
typedef boost::shared_ptr<MsckfVio> Ptr;
typedef boost::shared_ptr<const MsckfVio> ConstPtr;
- private:
+ public:
/*
* @brief StateServer Store one IMU states and several
* camera states for constructing measurement
@@ -202,15 +202,15 @@ class MsckfVio {
double tracking_rate_threshold;
// Ros node handle
- ros::NodeHandle nh;
+ ros::NodeHandle * nh;
// Subscribers and publishers
- ros::Subscriber imu_sub;
+ /*ros::Subscriber imu_sub;
ros::Subscriber feature_sub;
ros::Publisher odom_pub;
ros::Publisher feature_pub;
tf::TransformBroadcaster tf_pub;
- ros::ServiceServer reset_srv;
+ ros::ServiceServer reset_srv;*/
// Frame id
std::string fixed_frame_id;
@@ -228,8 +228,8 @@ class MsckfVio {
void mocapOdomCallback(
const nav_msgs::OdometryConstPtr& msg);
- ros::Subscriber mocap_odom_sub;
- ros::Publisher mocap_odom_pub;
+ /*ros::Subscriber mocap_odom_sub;
+ ros::Publisher mocap_odom_pub;*/
geometry_msgs::TransformStamped raw_mocap_odom_msg;
Eigen::Isometry3d mocap_initial_frame;
};
diff --git a/src/image_processor.cpp b/src/image_processor.cpp
index a5039f3..9877eb3 100644
--- a/src/image_processor.cpp
+++ b/src/image_processor.cpp
@@ -23,18 +23,18 @@ using namespace cv;
using namespace Eigen;
namespace msckf_vio {
-ImageProcessor::ImageProcessor(ros::NodeHandle& n) :
+ImageProcessor::ImageProcessor(ros::NodeHandle* n) :
nh(n),
is_first_img(true),
//img_transport(n),
- stereo_sub(10),
+ //stereo_sub(10),
prev_features_ptr(new GridFeatures()),
curr_features_ptr(new GridFeatures()) {
return;
}
ImageProcessor::~ImageProcessor() {
- destroyAllWindows();
+ //destroyAllWindows();
//ROS_INFO("Feature lifetime statistics:");
//featureLifetimeStatistics();
return;
@@ -42,37 +42,37 @@ ImageProcessor::~ImageProcessor() {
bool ImageProcessor::loadParameters() {
// Camera calibration parameters
- nh.param<string>("cam0/distortion_model",
+ nh->param<string>("cam0/distortion_model",
cam0_distortion_model, string("radtan"));
- nh.param<string>("cam1/distortion_model",
+ nh->param<string>("cam1/distortion_model",
cam1_distortion_model, string("radtan"));
vector<int> cam0_resolution_temp(2);
- nh.getParam("cam0/resolution", cam0_resolution_temp);
+ nh->getParam("cam0/resolution", cam0_resolution_temp);
cam0_resolution[0] = cam0_resolution_temp[0];
cam0_resolution[1] = cam0_resolution_temp[1];
vector<int> cam1_resolution_temp(2);
- nh.getParam("cam1/resolution", cam1_resolution_temp);
+ nh->getParam("cam1/resolution", cam1_resolution_temp);
cam1_resolution[0] = cam1_resolution_temp[0];
cam1_resolution[1] = cam1_resolution_temp[1];
vector<double> cam0_intrinsics_temp(4);
- nh.getParam("cam0/intrinsics", cam0_intrinsics_temp);
+ nh->getParam("cam0/intrinsics", cam0_intrinsics_temp);
cam0_intrinsics[0] = cam0_intrinsics_temp[0];
cam0_intrinsics[1] = cam0_intrinsics_temp[1];
cam0_intrinsics[2] = cam0_intrinsics_temp[2];
cam0_intrinsics[3] = cam0_intrinsics_temp[3];
vector<double> cam1_intrinsics_temp(4);
- nh.getParam("cam1/intrinsics", cam1_intrinsics_temp);
+ nh->getParam("cam1/intrinsics", cam1_intrinsics_temp);
cam1_intrinsics[0] = cam1_intrinsics_temp[0];
cam1_intrinsics[1] = cam1_intrinsics_temp[1];
cam1_intrinsics[2] = cam1_intrinsics_temp[2];
cam1_intrinsics[3] = cam1_intrinsics_temp[3];
vector<double> cam0_distortion_coeffs_temp(4);
- nh.getParam("cam0/distortion_coeffs",
+ nh->getParam("cam0/distortion_coeffs",
cam0_distortion_coeffs_temp);
cam0_distortion_coeffs[0] = cam0_distortion_coeffs_temp[0];
cam0_distortion_coeffs[1] = cam0_distortion_coeffs_temp[1];
@@ -80,20 +80,20 @@ bool ImageProcessor::loadParameters() {
cam0_distortion_coeffs[3] = cam0_distortion_coeffs_temp[3];
vector<double> cam1_distortion_coeffs_temp(4);
- nh.getParam("cam1/distortion_coeffs",
+ nh->getParam("cam1/distortion_coeffs",
cam1_distortion_coeffs_temp);
cam1_distortion_coeffs[0] = cam1_distortion_coeffs_temp[0];
cam1_distortion_coeffs[1] = cam1_distortion_coeffs_temp[1];
cam1_distortion_coeffs[2] = cam1_distortion_coeffs_temp[2];
cam1_distortion_coeffs[3] = cam1_distortion_coeffs_temp[3];
- cv::Mat T_imu_cam0 = utils::getTransformCV(nh, "cam0/T_cam_imu");
+ cv::Mat T_imu_cam0 = utils::getTransformCV(*nh, "cam0/T_cam_imu");
cv::Matx33d R_imu_cam0(T_imu_cam0(cv::Rect(0,0,3,3)));
cv::Vec3d t_imu_cam0 = T_imu_cam0(cv::Rect(3,0,1,3));
R_cam0_imu = R_imu_cam0.t();
t_cam0_imu = -R_imu_cam0.t() * t_imu_cam0;
- cv::Mat T_cam0_cam1 = utils::getTransformCV(nh, "cam1/T_cn_cnm1");
+ cv::Mat T_cam0_cam1 = utils::getTransformCV(*nh, "cam1/T_cn_cnm1");
cv::Mat T_imu_cam1 = T_cam0_cam1 * T_imu_cam0;
cv::Matx33d R_imu_cam1(T_imu_cam1(cv::Rect(0,0,3,3)));
cv::Vec3d t_imu_cam1 = T_imu_cam1(cv::Rect(3,0,1,3));
@@ -101,25 +101,25 @@ bool ImageProcessor::loadParameters() {
t_cam1_imu = -R_imu_cam1.t() * t_imu_cam1;
// Processor parameters
- nh.param<int>("grid_row", processor_config.grid_row, 4);
- nh.param<int>("grid_col", processor_config.grid_col, 4);
- nh.param<int>("grid_min_feature_num",
+ nh->param<int>("grid_row", processor_config.grid_row, 4);
+ nh->param<int>("grid_col", processor_config.grid_col, 4);
+ nh->param<int>("grid_min_feature_num",
processor_config.grid_min_feature_num, 2);
- nh.param<int>("grid_max_feature_num",
+ nh->param<int>("grid_max_feature_num",
processor_config.grid_max_feature_num, 4);
- nh.param<int>("pyramid_levels",
+ nh->param<int>("pyramid_levels",
processor_config.pyramid_levels, 3);
- nh.param<int>("patch_size",
+ nh->param<int>("patch_size",
processor_config.patch_size, 31);
- nh.param<int>("fast_threshold",
+ nh->param<int>("fast_threshold",
processor_config.fast_threshold, 20);
- nh.param<int>("max_iteration",
+ nh->param<int>("max_iteration",
processor_config.max_iteration, 30);
- nh.param<double>("track_precision",
+ nh->param<double>("track_precision",
processor_config.track_precision, 0.01);
- nh.param<double>("ransac_threshold",
+ nh->param<double>("ransac_threshold",
processor_config.ransac_threshold, 3);
- nh.param<double>("stereo_threshold",
+ nh->param<double>("stereo_threshold",
processor_config.stereo_threshold, 3);
ROS_INFO("===========================================");
@@ -175,19 +175,19 @@ bool ImageProcessor::loadParameters() {
}
bool ImageProcessor::createRosIO() {
- feature_pub = nh.advertise<CameraMeasurement>(
+ /* feature_pub = nh->advertise<CameraMeasurement>(
"features", 3);
- tracking_info_pub = nh.advertise<TrackingInfo>(
+ tracking_info_pub = nh->advertise<TrackingInfo>(
"tracking_info", 1);
- image_transport::ImageTransport it(nh);
+ image_transport::ImageTransport it(*nh);
debug_stereo_pub = it.advertise("debug_stereo_image", 1);
- cam0_img_sub.subscribe(nh, "cam0_image", 10);
- cam1_img_sub.subscribe(nh, "cam1_image", 10);
+ cam0_img_sub.subscribe(*nh, "cam0_image", 10);
+ cam1_img_sub.subscribe(*nh, "cam1_image", 10);
stereo_sub.connectInput(cam0_img_sub, cam1_img_sub);
stereo_sub.registerCallback(&ImageProcessor::stereoCallback, this);
- imu_sub = nh.subscribe("imu", 50,
- &ImageProcessor::imuCallback, this);
+ imu_sub = nh->subscribe("imu", 50,
+ &ImageProcessor::imuCallback, this);*/
return true;
}
@@ -590,12 +590,12 @@ void ImageProcessor::trackFeatures() {
for (const auto& item : *curr_features_ptr)
curr_feature_num += item.second.size();
- ROS_INFO_THROTTLE(0.5,
+ /*ROS_INFO_THROTTLE(0.5,
"\033[0;32m candidates: %d; track: %d; match: %d; ransac: %d/%d=%f\033[0m",
before_tracking, after_tracking, after_matching,
curr_feature_num, prev_feature_num,
static_cast<double>(curr_feature_num)/
- (static_cast<double>(prev_feature_num)+1e-5));
+ (static_cast<double>(prev_feature_num)+1e-5));*/
//printf(
// "\033[0;32m candidates: %d; raw track: %d; stereo match: %d; ransac: %d/%d=%f\033[0m\n",
// before_tracking, after_tracking, after_matching,
@@ -1266,7 +1266,7 @@ void ImageProcessor::publish() {
feature_msg_ptr->features[i].v1 = curr_cam1_points_undistorted[i].y;
}
- feature_pub.publish(feature_msg_ptr);
+ /* feature_pub.publish(feature_msg_ptr);*/
// Publish tracking info.
TrackingInfoPtr tracking_info_msg_ptr(new TrackingInfo());
@@ -1275,7 +1275,7 @@ void ImageProcessor::publish() {
tracking_info_msg_ptr->after_tracking = after_tracking;
tracking_info_msg_ptr->after_matching = after_matching;
tracking_info_msg_ptr->after_ransac = after_ransac;
- tracking_info_pub.publish(tracking_info_msg_ptr);
+ /*tracking_info_pub.publish(tracking_info_msg_ptr);*/
return;
}
@@ -1352,7 +1352,7 @@ void ImageProcessor::drawFeaturesMono() {
void ImageProcessor::drawFeaturesStereo() {
- if(debug_stereo_pub.getNumSubscribers() > 0)
+ /*if(debug_stereo_pub.getNumSubscribers() > 0)
{
// Colors for different features.
Scalar tracked(0, 255, 0);
@@ -1449,7 +1449,7 @@ void ImageProcessor::drawFeaturesStereo() {
}
//imshow("Feature", out_img);
//waitKey(5);
-
+*/
return;
}
diff --git a/src/image_processor_nodelet.cpp b/src/image_processor_nodelet.cpp
index 6deb1ed..f820543 100644
--- a/src/image_processor_nodelet.cpp
+++ b/src/image_processor_nodelet.cpp
@@ -9,7 +9,7 @@
namespace msckf_vio {
void ImageProcessorNodelet::onInit() {
- img_processor_ptr.reset(new ImageProcessor(getPrivateNodeHandle()));
+ img_processor_ptr.reset(new ImageProcessor(&getPrivateNodeHandle()));
if (!img_processor_ptr->initialize()) {
ROS_ERROR("Cannot initialize Image Processor...");
return;
diff --git a/src/msckf_vio.cpp b/src/msckf_vio.cpp
index 292e297..aaf84f9 100644
--- a/src/msckf_vio.cpp
+++ b/src/msckf_vio.cpp
@@ -50,7 +50,7 @@ Feature::OptimizationConfig Feature::optimization_config;
map<int, double> MsckfVio::chi_squared_test_table;
-MsckfVio::MsckfVio(ros::NodeHandle& pnh):
+MsckfVio::MsckfVio(ros::NodeHandle* pnh):
is_gravity_set(false),
is_first_img(true),
nh(pnh) {
@@ -59,26 +59,26 @@ MsckfVio::MsckfVio(ros::NodeHandle& pnh):
bool MsckfVio::loadParameters() {
// Frame id
- nh.param<string>("fixed_frame_id", fixed_frame_id, "world");
- nh.param<string>("child_frame_id", child_frame_id, "robot");
- nh.param<bool>("publish_tf", publish_tf, true);
- nh.param<double>("frame_rate", frame_rate, 40.0);
- nh.param<double>("position_std_threshold", position_std_threshold, 8.0);
+ nh->param<string>("fixed_frame_id", fixed_frame_id, "world");
+ nh->param<string>("child_frame_id", child_frame_id, "robot");
+ nh->param<bool>("publish_tf", publish_tf, true);
+ nh->param<double>("frame_rate", frame_rate, 40.0);
+ nh->param<double>("position_std_threshold", position_std_threshold, 8.0);
- nh.param<double>("rotation_threshold", rotation_threshold, 0.2618);
- nh.param<double>("translation_threshold", translation_threshold, 0.4);
- nh.param<double>("tracking_rate_threshold", tracking_rate_threshold, 0.5);
+ nh->param<double>("rotation_threshold", rotation_threshold, 0.2618);
+ nh->param<double>("translation_threshold", translation_threshold, 0.4);
+ nh->param<double>("tracking_rate_threshold", tracking_rate_threshold, 0.5);
// Feature optimization parameters
- nh.param<double>("feature/config/translation_threshold",
+ nh->param<double>("feature/config/translation_threshold",
Feature::optimization_config.translation_threshold, 0.2);
// Noise related parameters
- nh.param<double>("noise/gyro", IMUState::gyro_noise, 0.001);
- nh.param<double>("noise/acc", IMUState::acc_noise, 0.01);
- nh.param<double>("noise/gyro_bias", IMUState::gyro_bias_noise, 0.001);
- nh.param<double>("noise/acc_bias", IMUState::acc_bias_noise, 0.01);
- nh.param<double>("noise/feature", Feature::observation_noise, 0.01);
+ nh->param<double>("noise/gyro", IMUState::gyro_noise, 0.001);
+ nh->param<double>("noise/acc", IMUState::acc_noise, 0.01);
+ nh->param<double>("noise/gyro_bias", IMUState::gyro_bias_noise, 0.001);
+ nh->param<double>("noise/acc_bias", IMUState::acc_bias_noise, 0.01);
+ nh->param<double>("noise/feature", Feature::observation_noise, 0.01);
// Use variance instead of standard deviation.
IMUState::gyro_noise *= IMUState::gyro_noise;
@@ -92,28 +92,28 @@ bool MsckfVio::loadParameters() {
// implicitly. But the initial velocity and bias can be
// set by parameters.
// TODO: is it reasonable to set the initial bias to 0?
- nh.param<double>("initial_state/velocity/x",
+ nh->param<double>("initial_state/velocity/x",
state_server.imu_state.velocity(0), 0.0);
- nh.param<double>("initial_state/velocity/y",
+ nh->param<double>("initial_state/velocity/y",
state_server.imu_state.velocity(1), 0.0);
- nh.param<double>("initial_state/velocity/z",
+ nh->param<double>("initial_state/velocity/z",
state_server.imu_state.velocity(2), 0.0);
// The initial covariance of orientation and position can be
// set to 0. But for velocity, bias and extrinsic parameters,
// there should be nontrivial uncertainty.
double gyro_bias_cov, acc_bias_cov, velocity_cov;
- nh.param<double>("initial_covariance/velocity",
+ nh->param<double>("initial_covariance/velocity",
velocity_cov, 0.25);
- nh.param<double>("initial_covariance/gyro_bias",
+ nh->param<double>("initial_covariance/gyro_bias",
gyro_bias_cov, 1e-4);
- nh.param<double>("initial_covariance/acc_bias",
+ nh->param<double>("initial_covariance/acc_bias",
acc_bias_cov, 1e-2);
double extrinsic_rotation_cov, extrinsic_translation_cov;
- nh.param<double>("initial_covariance/extrinsic_rotation_cov",
+ nh->param<double>("initial_covariance/extrinsic_rotation_cov",
extrinsic_rotation_cov, 3.0462e-4);
- nh.param<double>("initial_covariance/extrinsic_translation_cov",
+ nh->param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
state_server.state_cov = MatrixXd::Zero(21, 21);
@@ -129,18 +129,18 @@ bool MsckfVio::loadParameters() {
state_server.state_cov(i, i) = extrinsic_translation_cov;
// Transformation offsets between the frames involved.
- Isometry3d T_imu_cam0 = utils::getTransformEigen(nh, "cam0/T_cam_imu");
+ Isometry3d T_imu_cam0 = utils::getTransformEigen(*nh, "cam0/T_cam_imu");
Isometry3d T_cam0_imu = T_imu_cam0.inverse();
state_server.imu_state.R_imu_cam0 = T_cam0_imu.linear().transpose();
state_server.imu_state.t_cam0_imu = T_cam0_imu.translation();
CAMState::T_cam0_cam1 =
- utils::getTransformEigen(nh, "cam1/T_cn_cnm1");
+ utils::getTransformEigen(*nh, "cam1/T_cn_cnm1");
IMUState::T_imu_body =
- utils::getTransformEigen(nh, "T_imu_body").inverse();
+ utils::getTransformEigen(*nh, "T_imu_body").inverse();
// Maximum number of camera states to be stored
- nh.param<int>("max_cam_state_size", max_cam_state_size, 30);
+ nh->param<int>("max_cam_state_size", max_cam_state_size, 30);
ROS_INFO("===========================================");
ROS_INFO("fixed frame id: %s", fixed_frame_id.c_str());
@@ -177,21 +177,21 @@ bool MsckfVio::loadParameters() {
}
bool MsckfVio::createRosIO() {
- odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
- feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
+ /*odom_pub = nh->advertise<nav_msgs::Odometry>("odom", 10);
+ feature_pub = nh->advertise<sensor_msgs::PointCloud2>(
"feature_point_cloud", 10);
- reset_srv = nh.advertiseService("reset",
+ reset_srv = nh->advertiseService("reset",
&MsckfVio::resetCallback, this);
- imu_sub = nh.subscribe("imu", 100,
+ imu_sub = nh->subscribe("imu", 100,
&MsckfVio::imuCallback, this);
- feature_sub = nh.subscribe("features", 40,
+ feature_sub = nh->subscribe("features", 40,
&MsckfVio::featureCallback, this);
- mocap_odom_sub = nh.subscribe("mocap_odom", 10,
+ mocap_odom_sub = nh->subscribe("mocap_odom", 10,
&MsckfVio::mocapOdomCallback, this);
- mocap_odom_pub = nh.advertise<nav_msgs::Odometry>("gt_odom", 1);
+ mocap_odom_pub = nh->advertise<nav_msgs::Odometry>("gt_odom", 1);*/
return true;
}
@@ -217,7 +217,7 @@ bool MsckfVio::initialize() {
for (int i = 1; i < 100; ++i) {
boost::math::chi_squared chi_squared_dist(i);
chi_squared_test_table[i] =
- boost::math::quantile(chi_squared_dist, 0.05);
+ boost::math::quantile(chi_squared_dist, 0.95);
}
if (!createRosIO()) return false;
@@ -275,10 +275,12 @@ void MsckfVio::initializeGravityAndBias() {
double gravity_norm = gravity_imu.norm();
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);
- Quaterniond q0_i_w = Quaterniond::FromTwoVectors(
- gravity_imu, -IMUState::gravity);
- state_server.imu_state.orientation =
- rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());
+ Eigen::Isometry3d T_b_w = Isometry3d::Identity();
+ T_b_w.linear() = Quaterniond::FromTwoVectors(
+ IMUState::T_imu_body.linear() * gravity_imu, -IMUState::gravity).toRotationMatrix();
+ Eigen::Isometry3d T_i_w = T_b_w * IMUState::T_imu_body;
+ state_server.imu_state.orientation = rotationToQuaternion(T_i_w.linear().transpose());
+ state_server.imu_state.position = T_i_w.translation();
return;
}
@@ -290,8 +292,8 @@ bool MsckfVio::resetCallback(
ROS_WARN("Start resetting msckf vio...");
// Temporarily shutdown the subscribers to prevent the
// state from updating.
- feature_sub.shutdown();
- imu_sub.shutdown();
+ /* feature_sub.shutdown();
+ imu_sub.shutdown();*/
// Reset the IMU state.
IMUState& imu_state = state_server.imu_state;
@@ -310,17 +312,17 @@ bool MsckfVio::resetCallback(
// Reset the state covariance.
double gyro_bias_cov, acc_bias_cov, velocity_cov;
- nh.param<double>("initial_covariance/velocity",
+ nh->param<double>("initial_covariance/velocity",
velocity_cov, 0.25);
- nh.param<double>("initial_covariance/gyro_bias",
+ nh->param<double>("initial_covariance/gyro_bias",
gyro_bias_cov, 1e-4);
- nh.param<double>("initial_covariance/acc_bias",
+ nh->param<double>("initial_covariance/acc_bias",
acc_bias_cov, 1e-2);
double extrinsic_rotation_cov, extrinsic_translation_cov;
- nh.param<double>("initial_covariance/extrinsic_rotation_cov",
+ nh->param<double>("initial_covariance/extrinsic_rotation_cov",
extrinsic_rotation_cov, 3.0462e-4);
- nh.param<double>("initial_covariance/extrinsic_translation_cov",
+ nh->param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
state_server.state_cov = MatrixXd::Zero(21, 21);
@@ -346,10 +348,10 @@ bool MsckfVio::resetCallback(
is_first_img = true;
// Restart the subscribers.
- imu_sub = nh.subscribe("imu", 100,
+ /*imu_sub = nh->subscribe("imu", 100,
&MsckfVio::imuCallback, this);
- feature_sub = nh.subscribe("features", 40,
- &MsckfVio::featureCallback, this);
+ feature_sub = nh->subscribe("features", 40,
+ &MsckfVio::featureCallback, this);*/
// TODO: When can the reset fail?
res.success = true;
@@ -484,12 +486,12 @@ void MsckfVio::mocapOdomCallback(
// body_velocity_gt;
// Ground truth tf.
- if (publish_tf) {
+ /*if (publish_tf) {
tf::Transform T_b_w_gt_tf;
tf::transformEigenToTF(T_b_w_gt, T_b_w_gt_tf);
tf_pub.sendTransform(tf::StampedTransform(
T_b_w_gt_tf, msg->header.stamp, fixed_frame_id, child_frame_id+"_mocap"));
- }
+ }*/
// Ground truth odometry.
nav_msgs::Odometry mocap_odom_msg;
@@ -501,7 +503,7 @@ void MsckfVio::mocapOdomCallback(
//tf::vectorEigenToMsg(body_velocity_gt,
// mocap_odom_msg.twist.twist.linear);
- mocap_odom_pub.publish(mocap_odom_msg);
+ /* mocap_odom_pub.publish(mocap_odom_msg);*/
return;
}
@@ -1334,17 +1336,17 @@ void MsckfVio::onlineReset() {
// Reset the state covariance.
double gyro_bias_cov, acc_bias_cov, velocity_cov;
- nh.param<double>("initial_covariance/velocity",
+ nh->param<double>("initial_covariance/velocity",
velocity_cov, 0.25);
- nh.param<double>("initial_covariance/gyro_bias",
+ nh->param<double>("initial_covariance/gyro_bias",
gyro_bias_cov, 1e-4);
- nh.param<double>("initial_covariance/acc_bias",
+ nh->param<double>("initial_covariance/acc_bias",
acc_bias_cov, 1e-2);
double extrinsic_rotation_cov, extrinsic_translation_cov;
- nh.param<double>("initial_covariance/extrinsic_rotation_cov",
+ nh->param<double>("initial_covariance/extrinsic_rotation_cov",
extrinsic_rotation_cov, 3.0462e-4);
- nh.param<double>("initial_covariance/extrinsic_translation_cov",
+ nh->param<double>("initial_covariance/extrinsic_translation_cov",
extrinsic_translation_cov, 1e-4);
state_server.state_cov = MatrixXd::Zero(21, 21);
@@ -1372,19 +1374,18 @@ void MsckfVio::publish(const ros::Time& time) {
imu_state.orientation).transpose();
T_i_w.translation() = imu_state.position;
- Eigen::Isometry3d T_b_w = IMUState::T_imu_body * T_i_w *
- IMUState::T_imu_body.inverse();
+ Eigen::Isometry3d T_b_w = T_i_w * IMUState::T_imu_body.inverse();
Eigen::Vector3d body_velocity =
IMUState::T_imu_body.linear() * imu_state.velocity;
// Publish tf
- if (publish_tf) {
+ /*if (publish_tf) {
tf::Transform T_b_w_tf;
tf::transformEigenToTF(T_b_w, T_b_w_tf);
tf_pub.sendTransform(tf::StampedTransform(
T_b_w_tf, time, fixed_frame_id, child_frame_id));
}
-
+*/
// Publish the odometry
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = time;
@@ -1420,7 +1421,7 @@ void MsckfVio::publish(const ros::Time& time) {
for (int j = 0; j < 3; ++j)
odom_msg.twist.covariance[i*6+j] = P_body_vel(i, j);
- odom_pub.publish(odom_msg);
+ /*odom_pub.publish(odom_msg);*/
// Publish the 3D positions of the features that
// has been initialized.
@@ -1431,15 +1432,13 @@ void MsckfVio::publish(const ros::Time& time) {
for (const auto& item : map_server) {
const auto& feature = item.second;
if (feature.is_initialized) {
- Vector3d feature_position =
- IMUState::T_imu_body.linear() * feature.position;
feature_msg_ptr->points.push_back(pcl::PointXYZ(
- feature_position(0), feature_position(1), feature_position(2)));
+ feature.position(0), feature.position(1), feature.position(2)));
}
}
feature_msg_ptr->width = feature_msg_ptr->points.size();
- feature_pub.publish(feature_msg_ptr);
+ /*feature_pub.publish(feature_msg_ptr);*/
return;
}
diff --git a/src/msckf_vio_nodelet.cpp b/src/msckf_vio_nodelet.cpp
index f8eef5e..7d536d2 100644
--- a/src/msckf_vio_nodelet.cpp
+++ b/src/msckf_vio_nodelet.cpp
@@ -9,7 +9,7 @@
namespace msckf_vio {
void MsckfVioNodelet::onInit() {
- msckf_vio_ptr.reset(new MsckfVio(getPrivateNodeHandle()));
+ msckf_vio_ptr.reset(new MsckfVio(&getPrivateNodeHandle()));
if (!msckf_vio_ptr->initialize()) {
ROS_ERROR("Cannot initialize MSCKF VIO...");
return;
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment