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

Fix odometry in the 2D pose graph optimization. (#533) 

Before, the relative change in pose due to odometry in the
2D pose graph optimization problem was incorrectly done in
the tracking frame. Instead, the gravity aligned frame in
which the 2D poses are stored must be used.

Fixes #515.

PAIR=cschuet
master
Wolfgang Hess 2017-09-15 14:11:37 +02:00 committed by GitHub
parent 6b544628b6
commit 0cbc420b02
3 changed files with 19 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -250,9 +250,10 @@ void SparsePoseGraph::ComputeConstraintsForScan(
->first + ->first +
1) 1)
: 0}; : 0};
const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; const auto& node_data = trajectory_nodes_.at(node_id).constant_data;
optimization_problem_.AddTrajectoryNode( optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id,
matching_id.trajectory_id, scan_data->time, pose, optimized_pose); node_data->time, pose, optimized_pose,
node_data->gravity_alignment);
for (size_t i = 0; i < insertion_submaps.size(); ++i) { for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const mapping::SubmapId submap_id = submap_ids[i]; const mapping::SubmapId submap_id = submap_ids[i];
// Even if this was the last scan added to 'submap_id', the submap will only // Even if this was the last scan added to 'submap_id', the submap will only

17
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc
View File

@ -80,14 +80,16 @@ void OptimizationProblem::AddOdometerData(
void OptimizationProblem::AddTrajectoryNode( void OptimizationProblem::AddTrajectoryNode(
const int trajectory_id, const common::Time time, const int trajectory_id, const common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose) { const transform::Rigid2d& initial_pose, const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment) {
CHECK_GE(trajectory_id, 0); CHECK_GE(trajectory_id, 0);
node_data_.resize( node_data_.resize(
std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1)); std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size()));
auto& trajectory_data = trajectory_data_[trajectory_id]; auto& trajectory_data = trajectory_data_[trajectory_id];
node_data_[trajectory_id].emplace(trajectory_data.next_node_index, node_data_[trajectory_id].emplace(
NodeData{time, initial_pose, pose}); trajectory_data.next_node_index,
NodeData{time, initial_pose, pose, gravity_alignment});
++trajectory_data.next_node_index; ++trajectory_data.next_node_index;
} }
@ -227,8 +229,13 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
node_data_[trajectory_id][node_index].time); node_data_[trajectory_id][node_index].time);
const transform::Rigid3d relative_pose = const transform::Rigid3d relative_pose =
odometry_available odometry_available
? odometry_data_[trajectory_id].Lookup(node_data.time).inverse() * ? transform::Rigid3d::Rotation(node_data.gravity_alignment) *
odometry_data_[trajectory_id].Lookup(next_node_data.time) odometry_data_[trajectory_id]
.Lookup(node_data.time)
.inverse() *
odometry_data_[trajectory_id].Lookup(next_node_data.time) *
transform::Rigid3d::Rotation(
next_node_data.gravity_alignment.inverse())
: transform::Embed3D(node_data.initial_pose.inverse() * : transform::Embed3D(node_data.initial_pose.inverse() *
next_node_data.initial_pose); next_node_data.initial_pose);
problem.AddResidualBlock( problem.AddResidualBlock(

4
cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h
View File

@ -41,6 +41,7 @@ struct NodeData {
common::Time time; common::Time time;
transform::Rigid2d initial_pose; transform::Rigid2d initial_pose;
transform::Rigid2d pose; transform::Rigid2d pose;
Eigen::Quaterniond gravity_alignment;
}; };
struct SubmapData { struct SubmapData {
@ -65,7 +66,8 @@ class OptimizationProblem {
const sensor::OdometryData& odometry_data); const sensor::OdometryData& odometry_data);
void AddTrajectoryNode(int trajectory_id, common::Time time, void AddTrajectoryNode(int trajectory_id, common::Time time,
const transform::Rigid2d& initial_pose, const transform::Rigid2d& initial_pose,
const transform::Rigid2d& pose); const transform::Rigid2d& pose,
const Eigen::Quaterniond& gravity_alignment);
void TrimTrajectoryNode(const mapping::NodeId& node_id); void TrimTrajectoryNode(const mapping::NodeId& node_id);
void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose);
void TrimSubmap(const mapping::SubmapId& submap_id); void TrimSubmap(const mapping::SubmapId& submap_id);