From 8392a7e62cefca96ad9dcd388f5e7ce9958dcd2a Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Mon, 15 May 2017 16:31:59 +0200 Subject: [PATCH] Change SubmapState.scan_indices -> node_ids (#286) Related to #256. PAIR=wohe --- cartographer/mapping_2d/sparse_pose_graph.cc | 20 +++++++++-------- cartographer/mapping_2d/sparse_pose_graph.h | 4 ++-- cartographer/mapping_3d/sparse_pose_graph.cc | 23 ++++++++++---------- cartographer/mapping_3d/sparse_pose_graph.h | 4 ++-- 4 files changed, 27 insertions(+), 24 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 3dfc3b3..c5a96da 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -215,7 +215,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); const int num_nodes = scan_index_to_node_id_.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { - if (submap_state.scan_indices.count(scan_index) == 0) { + if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) { ComputeConstraint(scan_index, submap_id); } } @@ -235,9 +235,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( .pose * sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; CHECK_EQ(scan_index, scan_index_to_node_id_.size()); - scan_index_to_node_id_.push_back( - mapping::NodeId{matching_id.trajectory_id, - num_nodes_in_trajectory_[matching_id.trajectory_id]}); + const mapping::NodeId node_id{ + matching_id.trajectory_id, + num_nodes_in_trajectory_[matching_id.trajectory_id]}; + scan_index_to_node_id_.push_back(node_id); ++num_nodes_in_trajectory_[matching_id.trajectory_id]; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; @@ -252,7 +253,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( .finished); submap_states_.at(submap_id.trajectory_id) .at(submap_id.submap_index) - .scan_indices.emplace(scan_index); + .node_ids.emplace(node_id); // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid2d constraint_transform = sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose; @@ -260,7 +261,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( constexpr double kFakeOrientationCovariance = 1e-6; constraints_.push_back(Constraint{ submap_id, - scan_index_to_node_id_.at(scan_index), + node_id, {transform::Embed3D(constraint_transform), common::ComputeSpdMatrixSqrtInverse( kalman_filter::Embed3D(covariance, kFakePositionCovariance, @@ -278,7 +279,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( if (submap_states_.at(trajectory_id).at(submap_index).finished) { CHECK_EQ(submap_states_.at(trajectory_id) .at(submap_index) - .scan_indices.count(scan_index), + .node_ids.count(node_id), 0); ComputeConstraint(scan_index, mapping::SubmapId{static_cast(trajectory_id), @@ -388,9 +389,10 @@ void SparsePoseGraph::RunOptimization() { const auto& node_data = optimization_problem_.node_data(); const size_t num_optimized_poses = scan_index_to_node_id_.size(); for (size_t i = 0; i != num_optimized_poses; ++i) { + const mapping::NodeId node_id = scan_index_to_node_id_[i]; trajectory_nodes_[i].pose = - transform::Embed3D(node_data.at(scan_index_to_node_id_[i].trajectory_id) - .at(scan_index_to_node_id_[i].node_index) + transform::Embed3D(node_data.at(node_id.trajectory_id) + .at(node_id.node_index) .point_cloud_pose); } // Extrapolate all point cloud poses that were added later. diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index bb615ed..aa4f75c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -98,10 +98,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { struct SubmapState { const mapping::Submap* submap = nullptr; - // Indices of the scans that were inserted into this map together with + // IDs of the scans that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. - std::set scan_indices; + std::set node_ids; // Whether in the current state of the background thread this submap is // finished. When this transitions to true, all scans are tried to match diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index b7c37ee..29e4549 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -211,7 +211,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { submap_states_.at(submap_id.trajectory_id).at(submap_id.submap_index); const int num_nodes = scan_index_to_node_id_.size(); for (int scan_index = 0; scan_index < num_nodes; ++scan_index) { - if (submap_state.scan_indices.count(scan_index) == 0) { + if (submap_state.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) { ComputeConstraint(scan_index, submap_id); } } @@ -231,9 +231,10 @@ void SparsePoseGraph::ComputeConstraintsForScan( .pose * matching_submap->local_pose().inverse() * pose; CHECK_EQ(scan_index, scan_index_to_node_id_.size()); - scan_index_to_node_id_.push_back( - mapping::NodeId{matching_id.trajectory_id, - num_nodes_in_trajectory_[matching_id.trajectory_id]}); + const mapping::NodeId node_id{ + matching_id.trajectory_id, + num_nodes_in_trajectory_[matching_id.trajectory_id]}; + scan_index_to_node_id_.push_back(node_id); ++num_nodes_in_trajectory_[matching_id.trajectory_id]; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_[scan_index].constant_data; @@ -248,13 +249,13 @@ void SparsePoseGraph::ComputeConstraintsForScan( .finished); submap_states_.at(submap_id.trajectory_id) .at(submap_id.submap_index) - .scan_indices.emplace(scan_index); + .node_ids.emplace(node_id); // Unchanged covariance as (submap <- map) is a translation. const transform::Rigid3d constraint_transform = submap->local_pose().inverse() * pose; constraints_.push_back( Constraint{submap_id, - scan_index_to_node_id_.at(scan_index), + node_id, {constraint_transform, common::ComputeSpdMatrixSqrtInverse( covariance, options_.constraint_builder_options() @@ -270,7 +271,7 @@ void SparsePoseGraph::ComputeConstraintsForScan( if (submap_states_.at(trajectory_id).at(submap_index).finished) { CHECK_EQ(submap_states_.at(trajectory_id) .at(submap_index) - .scan_indices.count(scan_index), + .node_ids.count(node_id), 0); ComputeConstraint(scan_index, mapping::SubmapId{static_cast(trajectory_id), @@ -380,10 +381,10 @@ void SparsePoseGraph::RunOptimization() { const auto& node_data = optimization_problem_.node_data(); const size_t num_optimized_poses = scan_index_to_node_id_.size(); for (size_t i = 0; i != num_optimized_poses; ++i) { - trajectory_nodes_[i].pose = - node_data.at(scan_index_to_node_id_[i].trajectory_id) - .at(scan_index_to_node_id_[i].node_index) - .point_cloud_pose; + const mapping::NodeId node_id = scan_index_to_node_id_[i]; + trajectory_nodes_[i].pose = node_data.at(node_id.trajectory_id) + .at(node_id.node_index) + .point_cloud_pose; } // Extrapolate all point cloud poses that were added later. std::unordered_map diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 807b6a9..a097f0c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -100,10 +100,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { struct SubmapState { const Submap* submap = nullptr; - // Indices of the scans that were inserted into this map together with + // IDs of the scans that were inserted into this map together with // constraints for them. They are not to be matched again when this submap // becomes 'finished'. - std::set scan_indices; + std::set node_ids; // Whether in the current state of the background thread this submap is // finished. When this transitions to true, all scans are tried to match