From e3b6f0afc5a78b461da2557fd19753ace271281c Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 1 Sep 2017 15:40:21 +0200 Subject: [PATCH] Expose complete poses in 2D SLAM. (#498) This changes the trajectory nodes to contain the complete 'pose' including 'tracking_to_tracking_2d' applied already. Similar for the 'zbar_ij' as it is returned by 'constraints()'. This allows 2D and 3D to be handled in the same way. --- cartographer/mapping/map_builder.cc | 4 +--- cartographer/mapping/sparse_pose_graph.cc | 9 +++------ cartographer/mapping/trajectory_node.h | 9 ++++----- cartographer/mapping_2d/sparse_pose_graph.cc | 19 ++++++++++++++++--- .../mapping_3d/local_trajectory_builder.cc | 2 +- 5 files changed, 25 insertions(+), 18 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index d913d3c..8635bff 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -183,9 +183,7 @@ void MapBuilder::SerializeState(io::ProtoStreamWriter* const writer) { range_data_proto->mutable_node_id()->set_node_index(node_index); const auto& data = *node_data[trajectory_id][node_index].constant_data; *range_data_proto->mutable_range_data() = - sensor::ToProto(sensor::Compress(sensor::TransformRangeData( - sensor::Decompress(data.range_data), - data.tracking_to_pose.inverse().cast()))); + sensor::ToProto(data.range_data); // TODO(whess): Only enable optionally? Resulting pbstream files will be // a lot larger now. writer->WriteProto(proto); diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 16bc8c8..763d210 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -84,8 +84,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { auto* node_proto = trajectory_proto->add_node(); node_proto->set_timestamp( common::ToUniversal(node.constant_data->time)); - *node_proto->mutable_pose() = transform::ToProto( - node.pose * node.constant_data->tracking_to_pose); + *node_proto->mutable_pose() = transform::ToProto(node.pose); } } @@ -107,10 +106,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { for (const auto& constraint : constraints()) { auto* const constraint_proto = proto.add_constraint(); - const auto& node = all_trajectory_nodes.at(constraint.node_id.trajectory_id) - .at(constraint.node_id.node_index); - *constraint_proto->mutable_relative_pose() = transform::ToProto( - constraint.pose.zbar_ij * node.constant_data->tracking_to_pose); + *constraint_proto->mutable_relative_pose() = + transform::ToProto(constraint.pose.zbar_ij); constraint_proto->set_translation_weight( constraint.pose.translation_weight); constraint_proto->set_rotation_weight(constraint.pose.rotation_weight); diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index d63415f..e12c81c 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -32,7 +32,7 @@ struct TrajectoryNode { struct Data { common::Time time; - // Range data in 'pose' frame. + // Range data in 'tracking' frame. Only used in 3D. sensor::CompressedRangeData range_data; // Used for loop closure in 2D: voxel filtered returns in 'pose' frame. @@ -42,10 +42,9 @@ struct TrajectoryNode { sensor::PointCloud high_resolution_point_cloud; sensor::PointCloud low_resolution_point_cloud; - // Transform from the 3D 'tracking' frame to the 'pose' frame of the range - // data, which contains roll, pitch and height for 2D. In 3D this is always - // identity. - transform::Rigid3d tracking_to_pose; + // Transforms the 'tracking' frame into a gravity-aligned 'tracking_2d' + // frame. Only used in 2D. + transform::Rigid3d tracking_to_tracking_2d; }; common::Time time() const { return constant_data->time; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ccfbcbb..fb7f9f1 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -98,7 +98,8 @@ void SparsePoseGraph::AddScan( const transform::Rigid2d& pose, const int trajectory_id, const std::vector>& insertion_submaps) { const transform::Rigid3d optimized_pose( - GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); + GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose) * + constant_data->tracking_to_tracking_2d); common::MutexLocker locker(&mutex_); trajectory_nodes_.Append( @@ -445,7 +446,8 @@ void SparsePoseGraph::RunOptimization() { ++node_data_index, ++node_index) { const mapping::NodeId node_id{trajectory_id, node_index}; trajectory_nodes_.at(node_id).pose = - transform::Embed3D(node_data[trajectory_id][node_data_index].pose); + transform::Embed3D(node_data[trajectory_id][node_data_index].pose) * + trajectory_nodes_.at(node_id).constant_data->tracking_to_tracking_2d; } // Extrapolate all point cloud poses that were added later. const auto local_to_new_global = @@ -482,8 +484,19 @@ SparsePoseGraph::GetTrajectoryNodes() { } std::vector SparsePoseGraph::constraints() { + std::vector result; common::MutexLocker locker(&mutex_); - return constraints_; + for (const Constraint& constraint : constraints_) { + result.push_back(Constraint{ + constraint.submap_id, constraint.node_id, + Constraint::Pose{constraint.pose.zbar_ij * + trajectory_nodes_.at(constraint.node_id) + .constant_data->tracking_to_tracking_2d, + constraint.pose.translation_weight, + constraint.pose.rotation_weight}, + constraint.tag}); + } + return result; } transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( diff --git a/cartographer/mapping_3d/local_trajectory_builder.cc b/cartographer/mapping_3d/local_trajectory_builder.cc index bb9540b..f61e4a5 100644 --- a/cartographer/mapping_3d/local_trajectory_builder.cc +++ b/cartographer/mapping_3d/local_trajectory_builder.cc @@ -213,7 +213,7 @@ LocalTrajectoryBuilder::InsertIntoSubmap( {}, // 'filtered_point_cloud' is only used in 2D. high_resolution_point_cloud, low_resolution_point_cloud, - transform::Rigid3d::Identity()}), + {}}), // 'tracking_to_tracking_2d' in only used in 2D. pose_observation, std::move(insertion_submaps)}); }