diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index df48824..9bbbe8c 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -21,6 +21,9 @@ options = { odom_frame = "odom", provide_odom_frame = true, use_odometry_data = false, + use_constant_odometry_variance = false, + constant_odometry_translational_variance = 0., + constant_odometry_rotational_variance = 0., publish_occupancy_grid = false, use_horizontal_laser = false, use_horizontal_multi_echo_laser = true, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 3eaf22c..9d7e644 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -21,6 +21,9 @@ options = { odom_frame = "odom", provide_odom_frame = true, use_odometry_data = false, + use_constant_odometry_variance = false, + constant_odometry_translational_variance = 0., + constant_odometry_rotational_variance = 0., publish_occupancy_grid = false, use_horizontal_laser = false, use_horizontal_multi_echo_laser = false, diff --git a/cartographer_ros/src/cartographer_node_main.cc b/cartographer_ros/src/cartographer_node_main.cc index e3f0750..e68d361 100644 --- a/cartographer_ros/src/cartographer_node_main.cc +++ b/cartographer_ros/src/cartographer_node_main.cc @@ -108,6 +108,9 @@ struct NodeOptions { bool publish_occupancy_grid; bool provide_odom_frame; bool use_odometry_data; + bool use_constant_odometry_variance; + double constant_odometry_translational_variance; + double constant_odometry_rotational_variance; bool use_horizontal_laser; bool use_horizontal_multi_echo_laser; double horizontal_laser_min_range; @@ -134,6 +137,12 @@ NodeOptions CreateNodeOptions( lua_parameter_dictionary->GetBool("provide_odom_frame"); options.use_odometry_data = lua_parameter_dictionary->GetBool("use_odometry_data"); + options.use_constant_odometry_variance = + lua_parameter_dictionary->GetBool("use_constant_odometry_variance"); + options.constant_odometry_translational_variance = + lua_parameter_dictionary->GetDouble("constant_odometry_translational_variance"); + options.constant_odometry_rotational_variance = + lua_parameter_dictionary->GetDouble("constant_odometry_rotational_variance"); options.use_horizontal_laser = lua_parameter_dictionary->GetBool("use_horizontal_laser"); options.use_horizontal_multi_echo_laser = @@ -277,8 +286,20 @@ Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time, void Node::AddOdometry(int64 timestamp, const string& frame_id, const Rigid3d& pose, const PoseCovariance& covariance) { const carto::common::Time time = carto::common::FromUniversal(timestamp); + PoseCovariance applied_covariance = covariance; + if (options_.use_constant_odometry_variance) { + const Eigen::Matrix3d translational = + Eigen::Matrix3d::Identity() * + options_.constant_odometry_translational_variance; + const Eigen::Matrix3d rotational = + Eigen::Matrix3d::Identity() * + options_.constant_odometry_rotational_variance; + applied_covariance << // + translational, Eigen::Matrix3d::Zero(), // + Eigen::Matrix3d::Zero(), rotational; + } map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId) - ->AddOdometerPose(time, pose, covariance); + ->AddOdometerPose(time, pose, applied_covariance); } void Node::AddImu(const int64 timestamp, const string& frame_id,