From 8612f5e6e518afc95fb85d5a2cf4642dd0e1c788 Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Tue, 16 May 2017 11:42:18 +0200 Subject: [PATCH] ConstantData now has a trajectory_id. (#289) Related to #256. PAIR=wohe --- cartographer/mapping/sparse_pose_graph.cc | 5 +- cartographer/mapping/sparse_pose_graph.h | 5 ++ cartographer/mapping/trajectory_node.h | 4 +- cartographer/mapping_2d/sparse_pose_graph.cc | 52 ++++++++++---------- cartographer/mapping_2d/sparse_pose_graph.h | 4 +- cartographer/mapping_3d/sparse_pose_graph.cc | 51 +++++++++---------- cartographer/mapping_3d/sparse_pose_graph.h | 4 +- 7 files changed, 67 insertions(+), 58 deletions(-) diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 0b66ef7..a420ae0 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -93,9 +93,8 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { } if (!trajectory_nodes.empty()) { - const Submaps* const trajectory = - trajectory_nodes[0].constant_data->trajectory; - for (const auto& transform : GetSubmapTransforms(trajectory)) { + for (const auto& transform : GetSubmapTransforms( + trajectory_nodes[0].constant_data->trajectory_id)) { *trajectory_proto->add_submap()->mutable_pose() = transform::ToProto(transform); } diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index 4194e98..66782b7 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -72,9 +72,14 @@ class SparsePoseGraph { virtual std::vector> GetConnectedTrajectories() = 0; // Returns the current optimized transforms for the given 'trajectory'. + // TODO(hrapp): Remove this version. virtual std::vector GetSubmapTransforms( const Submaps* trajectory) = 0; + // Returns the current optimized transforms for the given 'trajectory_id'. + virtual std::vector GetSubmapTransforms( + int trajectory_id) = 0; + // Returns the transform converting data in the local map frame (i.e. the // continuous, non-loop-closed frame) into the global map frame (i.e. the // discontinuous, loop-closed frame). diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index f0cf70a..dcb6697 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -41,9 +41,7 @@ struct TrajectoryNode { sensor::CompressedRangeData range_data_3d; // Trajectory this node belongs to. - // TODO(macmason): The naming here is confusing because 'trajectory' - // doesn't seem like a good name for a Submaps*. Sort this out. - const Submaps* trajectory; + int trajectory_id; // 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 diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 1c45224..4f4549f 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -105,8 +105,8 @@ void SparsePoseGraph::AddScan( constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, - Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), trajectory, - tracking_to_pose}); + Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), + trajectory_id, tracking_to_pose}); trajectory_nodes_.push_back(mapping::TrajectoryNode{ &constant_node_data_.back(), optimized_pose, }); @@ -174,9 +174,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, .at(node_id.node_index) .point_cloud_pose; - const mapping::Submaps* const scan_trajectory = - trajectory_nodes_[scan_index].constant_data->trajectory; - const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory); + const int scan_trajectory_id = + trajectory_nodes_[scan_index].constant_data->trajectory_id; // Only globally match against submaps not in this trajectory. if (scan_trajectory_id != submap_id.trajectory_id && @@ -242,8 +241,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( ++num_nodes_in_trajectory_[matching_id.trajectory_id]; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; - CHECK_EQ(trajectory_ids_.at(scan_data->trajectory), - matching_id.trajectory_id); + CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode( matching_id.trajectory_id, scan_data->time, pose, optimized_pose); for (const mapping::Submap* submap : insertion_submaps) { @@ -396,23 +394,21 @@ void SparsePoseGraph::RunOptimization() { .point_cloud_pose); } // Extrapolate all point cloud poses that were added later. - std::unordered_map - extrapolation_transforms; + std::unordered_map extrapolation_transforms; for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { - const mapping::Submaps* trajectory = - trajectory_nodes_[i].constant_data->trajectory; - if (extrapolation_transforms.count(trajectory) == 0) { + const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id; + if (extrapolation_transforms.count(trajectory_id) == 0) { const auto new_submap_transforms = ExtrapolateSubmapTransforms( - optimization_problem_.submap_data(), trajectory); - const auto old_submap_transforms = - ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); + optimization_problem_.submap_data(), trajectory_id); + const auto old_submap_transforms = ExtrapolateSubmapTransforms( + optimized_submap_transforms_, trajectory_id); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); - extrapolation_transforms[trajectory] = + extrapolation_transforms[trajectory_id] = transform::Rigid3d(new_submap_transforms.back() * old_submap_transforms.back().inverse()); } trajectory_nodes_[i].pose = - extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; + extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose; } optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); @@ -430,8 +426,7 @@ SparsePoseGraph::GetTrajectoryNodes() { std::vector> result( trajectory_ids_.size()); for (const auto& node : trajectory_nodes_) { - result.at(trajectory_ids_.at(node.constant_data->trajectory)) - .push_back(node); + result.at(node.constant_data->trajectory_id).push_back(node); } return result; } @@ -458,17 +453,24 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { std::vector SparsePoseGraph::GetSubmapTransforms( const mapping::Submaps* const trajectory) { common::MutexLocker locker(&mutex_); - return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); + if (trajectory_ids_.count(trajectory) == 0) { + return {transform::Rigid3d::Identity()}; + } + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, + trajectory_ids_.at(trajectory)); +} + +std::vector SparsePoseGraph::GetSubmapTransforms( + const int trajectory_id) { + common::MutexLocker locker(&mutex_); + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, + trajectory_id); } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - const mapping::Submaps* const trajectory) const { - if (trajectory_ids_.count(trajectory) == 0) { - return {transform::Rigid3d::Identity()}; - } - const size_t trajectory_id = trajectory_ids_.at(trajectory); + const size_t trajectory_id) const { if (trajectory_id >= submap_states_.size()) { return {transform::Rigid3d::Identity()}; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 8a08fae..444665c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -84,6 +84,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; + std::vector GetSubmapTransforms(int trajectory_id) + EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() @@ -153,7 +155,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - const mapping::Submaps* trajectory) const REQUIRES(mutex_); + size_t trajectory_id) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index c221097..135159e 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -103,7 +103,7 @@ void SparsePoseGraph::AddScan( constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, - sensor::Compress(range_data_in_tracking), trajectory, + sensor::Compress(range_data_in_tracking), trajectory_id, transform::Rigid3d::Identity()}); trajectory_nodes_.push_back( mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); @@ -173,10 +173,8 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, .at(node_id.trajectory_id) .at(node_id.node_index) .point_cloud_pose; - - const mapping::Submaps* const scan_trajectory = - trajectory_nodes_[scan_index].constant_data->trajectory; - const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory); + const int scan_trajectory_id = + trajectory_nodes_[scan_index].constant_data->trajectory_id; // Only globally match against submaps not in this trajectory. if (scan_trajectory_id != submap_id.trajectory_id && @@ -238,8 +236,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( ++num_nodes_in_trajectory_[matching_id.trajectory_id]; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; - CHECK_EQ(trajectory_ids_.at(scan_data->trajectory), - matching_id.trajectory_id); + CHECK_EQ(scan_data->trajectory_id, matching_id.trajectory_id); optimization_problem_.AddTrajectoryNode(matching_id.trajectory_id, scan_data->time, optimized_pose); for (const Submap* submap : insertion_submaps) { @@ -387,23 +384,21 @@ void SparsePoseGraph::RunOptimization() { .point_cloud_pose; } // Extrapolate all point cloud poses that were added later. - std::unordered_map - extrapolation_transforms; + std::unordered_map extrapolation_transforms; for (size_t i = num_optimized_poses; i != trajectory_nodes_.size(); ++i) { - const mapping::Submaps* trajectory = - trajectory_nodes_[i].constant_data->trajectory; - if (extrapolation_transforms.count(trajectory) == 0) { + const int trajectory_id = trajectory_nodes_[i].constant_data->trajectory_id; + if (extrapolation_transforms.count(trajectory_id) == 0) { const auto new_submap_transforms = ExtrapolateSubmapTransforms( - optimization_problem_.submap_data(), trajectory); - const auto old_submap_transforms = - ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); + optimization_problem_.submap_data(), trajectory_id); + const auto old_submap_transforms = ExtrapolateSubmapTransforms( + optimized_submap_transforms_, trajectory_id); CHECK_EQ(new_submap_transforms.size(), old_submap_transforms.size()); - extrapolation_transforms[trajectory] = + extrapolation_transforms[trajectory_id] = transform::Rigid3d(new_submap_transforms.back() * old_submap_transforms.back().inverse()); } trajectory_nodes_[i].pose = - extrapolation_transforms[trajectory] * trajectory_nodes_[i].pose; + extrapolation_transforms[trajectory_id] * trajectory_nodes_[i].pose; } optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); @@ -421,8 +416,7 @@ SparsePoseGraph::GetTrajectoryNodes() { std::vector> result( trajectory_ids_.size()); for (const auto& node : trajectory_nodes_) { - result.at(trajectory_ids_.at(node.constant_data->trajectory)) - .push_back(node); + result.at(node.constant_data->trajectory_id).push_back(node); } return result; } @@ -449,17 +443,24 @@ std::vector> SparsePoseGraph::GetConnectedTrajectories() { std::vector SparsePoseGraph::GetSubmapTransforms( const mapping::Submaps* const trajectory) { common::MutexLocker locker(&mutex_); - return ExtrapolateSubmapTransforms(optimized_submap_transforms_, trajectory); + if (trajectory_ids_.count(trajectory) == 0) { + return {transform::Rigid3d::Identity()}; + } + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, + trajectory_ids_.at(trajectory)); +} + +std::vector SparsePoseGraph::GetSubmapTransforms( + const int trajectory_id) { + common::MutexLocker locker(&mutex_); + return ExtrapolateSubmapTransforms(optimized_submap_transforms_, + trajectory_id); } std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - const mapping::Submaps* const trajectory) const { - if (trajectory_ids_.count(trajectory) == 0) { - return {transform::Rigid3d::Identity()}; - } - const size_t trajectory_id = trajectory_ids_.at(trajectory); + const size_t trajectory_id) const { if (trajectory_id >= submap_states_.size()) { return {transform::Rigid3d::Identity()}; } diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index b327126..a05f103 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -86,6 +86,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; + std::vector GetSubmapTransforms(int trajectory_id) + EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) EXCLUDES(mutex_) override; std::vector> GetTrajectoryNodes() @@ -153,7 +155,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, - const mapping::Submaps* trajectory) const REQUIRES(mutex_); + size_t trajectory_id) const REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_;