12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Adds an option for constant odometry variance. (#56)

master
Damon Kohler 2016-09-15 13:42:05 +02:00 committed by GitHub
parent 15e9638ce7
commit 3592b13871
3 changed files with 28 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cartographer_ros/configuration_files/backpack_2d.lua
View File

@ -21,6 +21,9 @@ options = {
odom_frame = "odom", odom_frame = "odom",
provide_odom_frame = true, provide_odom_frame = true,
use_odometry_data = false, use_odometry_data = false,
use_constant_odometry_variance = false,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
publish_occupancy_grid = false, publish_occupancy_grid = false,
use_horizontal_laser = false, use_horizontal_laser = false,
use_horizontal_multi_echo_laser = true, use_horizontal_multi_echo_laser = true,

3
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -21,6 +21,9 @@ options = {
odom_frame = "odom", odom_frame = "odom",
provide_odom_frame = true, provide_odom_frame = true,
use_odometry_data = false, use_odometry_data = false,
use_constant_odometry_variance = false,
constant_odometry_translational_variance = 0.,
constant_odometry_rotational_variance = 0.,
publish_occupancy_grid = false, publish_occupancy_grid = false,
use_horizontal_laser = false, use_horizontal_laser = false,
use_horizontal_multi_echo_laser = false, use_horizontal_multi_echo_laser = false,

23
cartographer_ros/src/cartographer_node_main.cc
View File

@ -108,6 +108,9 @@ struct NodeOptions {
bool publish_occupancy_grid; bool publish_occupancy_grid;
bool provide_odom_frame; bool provide_odom_frame;
bool use_odometry_data; 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_laser;
bool use_horizontal_multi_echo_laser; bool use_horizontal_multi_echo_laser;
double horizontal_laser_min_range; double horizontal_laser_min_range;
@ -134,6 +137,12 @@ NodeOptions CreateNodeOptions(
lua_parameter_dictionary->GetBool("provide_odom_frame"); lua_parameter_dictionary->GetBool("provide_odom_frame");
options.use_odometry_data = options.use_odometry_data =
lua_parameter_dictionary->GetBool("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 = options.use_horizontal_laser =
lua_parameter_dictionary->GetBool("use_horizontal_laser"); lua_parameter_dictionary->GetBool("use_horizontal_laser");
options.use_horizontal_multi_echo_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, void Node::AddOdometry(int64 timestamp, const string& frame_id,
const Rigid3d& pose, const PoseCovariance& covariance) { const Rigid3d& pose, const PoseCovariance& covariance) {
const carto::common::Time time = carto::common::FromUniversal(timestamp); 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) map_builder_.GetTrajectoryBuilder(kTrajectoryBuilderId)
->AddOdometerPose(time, pose, covariance); ->AddOdometerPose(time, pose, applied_covariance);
} }
void Node::AddImu(const int64 timestamp, const string& frame_id, void Node::AddImu(const int64 timestamp, const string& frame_id,