From 0bba56428f714512874a0fbdcc0e168561335bed Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 19 May 2017 14:01:59 +0200 Subject: [PATCH] Remove flat indices. (#299) This cleans up scan_index_to_node_id_, num_nodes_in_trajectory_, scan_index. Related to #256. --- cartographer/mapping_2d/sparse_pose_graph.cc | 35 ++++++++++-------- cartographer/mapping_2d/sparse_pose_graph.h | 7 +--- cartographer/mapping_3d/sparse_pose_graph.cc | 38 +++++++++++--------- cartographer/mapping_3d/sparse_pose_graph.h | 7 +--- 4 files changed, 43 insertions(+), 44 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index d699d39..1e237c2 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -97,9 +97,6 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose)); common::MutexLocker locker(&mutex_); - const int flat_scan_index = num_trajectory_nodes_; - CHECK_LT(flat_scan_index, std::numeric_limits::max()); - constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, range_data_in_pose, Compress(sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}), @@ -136,9 +133,8 @@ void SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(flat_scan_index, matching_submap, - insertion_submaps, finished_submap, pose, - covariance); + ComputeConstraintsForScan(matching_submap, insertion_submaps, + finished_submap, pose, covariance); }); } @@ -212,16 +208,23 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( const auto submap_id = GetSubmapId(submap); const auto& submap_state = 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.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) { - ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id); + + const auto& node_data = optimization_problem_.node_data(); + for (size_t trajectory_id = 0; trajectory_id != node_data.size(); + ++trajectory_id) { + for (size_t node_index = 0; node_index != node_data[trajectory_id].size(); + ++node_index) { + const mapping::NodeId node_id{static_cast(trajectory_id), + static_cast(node_index)}; + if (submap_state.node_ids.count(node_id) == 0) { + ComputeConstraint(node_id, submap_id); + } } } } void SparsePoseGraph::ComputeConstraintsForScan( - const int scan_index, const mapping::Submap* matching_submap, + const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) { @@ -233,12 +236,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.submap_index) .pose * sparse_pose_graph::ComputeSubmapPose(*matching_submap).inverse() * pose; - CHECK_EQ(scan_index, scan_index_to_node_id_.size()); 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]; + static_cast(matching_id.trajectory_id) < + optimization_problem_.node_data().size() + ? static_cast(optimization_problem_.node_data() + .at(matching_id.trajectory_id) + .size()) + : 0}; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_.at(node_id.trajectory_id) .at(node_id.node_index) diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 3422d76..a6291df 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -123,7 +123,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( - int scan_index, const mapping::Submap* matching_submap, + const mapping::Submap* matching_submap, std::vector insertion_submaps, const mapping::Submap* finished_submap, const transform::Rigid2d& pose, const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_); @@ -186,11 +186,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GUARDED_BY(mutex_); std::vector> submap_states_ GUARDED_BY(mutex_); - // Mapping to flat indices to aid the transition to per-trajectory data - // structures. - std::map num_nodes_in_trajectory_ GUARDED_BY(mutex_); - std::vector scan_index_to_node_id_ GUARDED_BY(mutex_); - // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; // Trajectory ID to connected component ID. diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 2f87f39..cce3a7b 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -96,9 +96,6 @@ void SparsePoseGraph::AddScan( GetLocalToGlobalTransform(trajectory_id) * pose); common::MutexLocker locker(&mutex_); - const int flat_scan_index = num_trajectory_nodes_; - CHECK_LT(flat_scan_index, std::numeric_limits::max()); - constant_node_data_.push_back(mapping::TrajectoryNode::ConstantData{ time, sensor::RangeData{Eigen::Vector3f::Zero(), {}, {}}, sensor::Compress(range_data_in_tracking), trajectory_id, @@ -132,9 +129,8 @@ void SparsePoseGraph::AddScan( } AddWorkItem([=]() REQUIRES(mutex_) { - ComputeConstraintsForScan(flat_scan_index, matching_submap, - insertion_submaps, finished_submap, pose, - covariance); + ComputeConstraintsForScan(matching_submap, insertion_submaps, + finished_submap, pose, covariance); }); } @@ -236,18 +232,24 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) { const auto submap_id = GetSubmapId(submap); const auto& submap_state = 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.node_ids.count(scan_index_to_node_id_[scan_index]) == 0) { - ComputeConstraint(scan_index_to_node_id_.at(scan_index), submap_id); + + const auto& node_data = optimization_problem_.node_data(); + for (size_t trajectory_id = 0; trajectory_id != node_data.size(); + ++trajectory_id) { + for (size_t node_index = 0; node_index != node_data[trajectory_id].size(); + ++node_index) { + const mapping::NodeId node_id{static_cast(trajectory_id), + static_cast(node_index)}; + if (submap_state.node_ids.count(node_id) == 0) { + ComputeConstraint(node_id, submap_id); + } } } } void SparsePoseGraph::ComputeConstraintsForScan( - const int scan_index, const Submap* matching_submap, - std::vector insertion_submaps, const Submap* finished_submap, - const transform::Rigid3d& pose, + const Submap* matching_submap, std::vector insertion_submaps, + const Submap* finished_submap, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) { GrowSubmapTransformsAsNeeded(insertion_submaps); const mapping::SubmapId matching_id = GetSubmapId(matching_submap); @@ -257,12 +259,14 @@ void SparsePoseGraph::ComputeConstraintsForScan( .at(matching_id.submap_index) .pose * matching_submap->local_pose.inverse() * pose; - CHECK_EQ(scan_index, scan_index_to_node_id_.size()); 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]; + static_cast(matching_id.trajectory_id) < + optimization_problem_.node_data().size() + ? static_cast(optimization_problem_.node_data() + .at(matching_id.trajectory_id) + .size()) + : 0}; const mapping::TrajectoryNode::ConstantData* const scan_data = trajectory_nodes_.at(node_id.trajectory_id) .at(node_id.node_index) diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 819d7a8..59804cd 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -121,7 +121,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Adds constraints for a scan, and starts scan matching in the background. void ComputeConstraintsForScan( - int scan_index, const Submap* matching_submap, + const Submap* matching_submap, std::vector insertion_submaps, const Submap* finished_submap, const transform::Rigid3d& pose, const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_); @@ -183,11 +183,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GUARDED_BY(mutex_); std::vector> submap_states_ GUARDED_BY(mutex_); - // Mapping to flat indices to aid the transition to per-trajectory data - // structures. - std::map num_nodes_in_trajectory_ GUARDED_BY(mutex_); - std::vector scan_index_to_node_id_ GUARDED_BY(mutex_); - // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; // Trajectory ID to connected component ID.