From 0cbc420b0267aaf4841c77c127165d32e37789f0 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 15 Sep 2017 14:11:37 +0200 Subject: [PATCH] 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 --- cartographer/mapping_2d/sparse_pose_graph.cc | 7 ++++--- .../sparse_pose_graph/optimization_problem.cc | 17 ++++++++++++----- .../sparse_pose_graph/optimization_problem.h | 4 +++- 3 files changed, 19 insertions(+), 9 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 6a09ede..c3ad9f1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -250,9 +250,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( ->first + 1) : 0}; - const auto& scan_data = trajectory_nodes_.at(node_id).constant_data; - optimization_problem_.AddTrajectoryNode( - matching_id.trajectory_id, scan_data->time, pose, optimized_pose); + const auto& node_data = trajectory_nodes_.at(node_id).constant_data; + optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, + node_data->time, pose, optimized_pose, + node_data->gravity_alignment); for (size_t i = 0; i < insertion_submaps.size(); ++i) { const mapping::SubmapId submap_id = submap_ids[i]; // Even if this was the last scan added to 'submap_id', the submap will only diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 49c47ac..27984d1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -80,14 +80,16 @@ void OptimizationProblem::AddOdometerData( void OptimizationProblem::AddTrajectoryNode( 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); node_data_.resize( std::max(node_data_.size(), static_cast(trajectory_id) + 1)); trajectory_data_.resize(std::max(trajectory_data_.size(), node_data_.size())); auto& trajectory_data = trajectory_data_[trajectory_id]; - node_data_[trajectory_id].emplace(trajectory_data.next_node_index, - NodeData{time, initial_pose, pose}); + node_data_[trajectory_id].emplace( + trajectory_data.next_node_index, + NodeData{time, initial_pose, pose, gravity_alignment}); ++trajectory_data.next_node_index; } @@ -227,8 +229,13 @@ void OptimizationProblem::Solve(const std::vector& constraints, node_data_[trajectory_id][node_index].time); const transform::Rigid3d relative_pose = odometry_available - ? odometry_data_[trajectory_id].Lookup(node_data.time).inverse() * - odometry_data_[trajectory_id].Lookup(next_node_data.time) + ? transform::Rigid3d::Rotation(node_data.gravity_alignment) * + 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() * next_node_data.initial_pose); problem.AddResidualBlock( diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h index c5f0b60..2af47b1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h @@ -41,6 +41,7 @@ struct NodeData { common::Time time; transform::Rigid2d initial_pose; transform::Rigid2d pose; + Eigen::Quaterniond gravity_alignment; }; struct SubmapData { @@ -65,7 +66,8 @@ class OptimizationProblem { const sensor::OdometryData& odometry_data); void AddTrajectoryNode(int trajectory_id, common::Time time, 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 AddSubmap(int trajectory_id, const transform::Rigid2d& submap_pose); void TrimSubmap(const mapping::SubmapId& submap_id);