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",
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,

3
cartographer_ros/configuration_files/backpack_3d.lua
View File

@ -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,

23
cartographer_ros/src/cartographer_node_main.cc
View File

@ -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,