From fa306d03ece7840968b5b340410a2016564c39d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 7 Sep 2017 15:58:30 +0200 Subject: [PATCH] Remove reverse_connected_components. (#507) Remove reverse_connected_components from SparsePoseGraphs and update TrajectoryConnectivity to return "connected" for the reflexive case even if trajectories are unknown. --- cartographer/mapping/trajectory_connectivity.cc | 4 ++++ cartographer/mapping/trajectory_connectivity.h | 7 ++++--- .../mapping/trajectory_connectivity_test.cc | 14 ++++++++++++-- cartographer/mapping_2d/sparse_pose_graph.cc | 15 ++------------- cartographer/mapping_2d/sparse_pose_graph.h | 2 -- cartographer/mapping_3d/sparse_pose_graph.cc | 15 ++------------- cartographer/mapping_3d/sparse_pose_graph.h | 2 -- 7 files changed, 24 insertions(+), 35 deletions(-) diff --git a/cartographer/mapping/trajectory_connectivity.cc b/cartographer/mapping/trajectory_connectivity.cc index e3def82..5d9d148 100644 --- a/cartographer/mapping/trajectory_connectivity.cc +++ b/cartographer/mapping/trajectory_connectivity.cc @@ -61,6 +61,10 @@ int TrajectoryConnectivity::FindSet(const int trajectory_id) { bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a, const int trajectory_id_b) { + if (trajectory_id_a == trajectory_id_b) { + return true; + } + common::MutexLocker locker(&lock_); if (forest_.count(trajectory_id_a) == 0 || diff --git a/cartographer/mapping/trajectory_connectivity.h b/cartographer/mapping/trajectory_connectivity.h index 890c90c..b3a6046 100644 --- a/cartographer/mapping/trajectory_connectivity.h +++ b/cartographer/mapping/trajectory_connectivity.h @@ -40,7 +40,7 @@ class TrajectoryConnectivity { TrajectoryConnectivity(const TrajectoryConnectivity&) = delete; TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete; - // Add a trajectory which is initially connected to nothing. + // Add a trajectory which is initially connected to only itself. void Add(int trajectory_id) EXCLUDES(lock_); // Connect two trajectories. If either trajectory is untracked, it will be @@ -49,8 +49,9 @@ class TrajectoryConnectivity { void Connect(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); // Determines if two trajectories have been (transitively) connected. If - // either trajectory is not being tracked, returns false. This function is - // invariant to the order of its arguments. + // either trajectory is not being tracked, returns false, except when it is + // the same trajectory, where it returns true. This function is invariant to + // the order of its arguments. bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); diff --git a/cartographer/mapping/trajectory_connectivity_test.cc b/cartographer/mapping/trajectory_connectivity_test.cc index 4d6c6b0..0c7d75a 100644 --- a/cartographer/mapping/trajectory_connectivity_test.cc +++ b/cartographer/mapping/trajectory_connectivity_test.cc @@ -35,8 +35,9 @@ TEST(TrajectoryConnectivityTest, TransitivelyConnected) { for (int trajectory_a = 0; trajectory_a < kNumTrajectories; ++trajectory_a) { for (int trajectory_b = 0; trajectory_b < kNumTrajectories; ++trajectory_b) { - EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a, - trajectory_b)); + EXPECT_EQ(trajectory_a == trajectory_b, + trajectory_connectivity.TransitivelyConnected(trajectory_a, + trajectory_b)); } } @@ -102,6 +103,15 @@ TEST(TrajectoryConnectivityTest, ConnectionCount) { } } +TEST(TrajectoryConnectivityTest, ReflexiveConnectivity) { + TrajectoryConnectivity trajectory_connectivity; + EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 0)); + EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(0, 0)); + trajectory_connectivity.Add(0); + EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 0)); + EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(0, 0)); +} + } // namespace } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 72e9479..4c7f465 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -180,13 +180,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get()); } else { - const bool scan_and_submap_trajectories_connected = - reverse_connected_components_.count(node_id.trajectory_id) > 0 && - reverse_connected_components_.count(submap_id.trajectory_id) > 0 && - reverse_connected_components_.at(node_id.trajectory_id) == - reverse_connected_components_.at(submap_id.trajectory_id); - if (node_id.trajectory_id == submap_id.trajectory_id || - scan_and_submap_trajectories_connected) { + if (trajectory_connectivity_.TransitivelyConnected( + node_id.trajectory_id, submap_id.trajectory_id)) { const transform::Rigid2d initial_relative_pose = optimization_problem_.submap_data() .at(submap_id.trajectory_id) @@ -471,12 +466,6 @@ void SparsePoseGraph::RunOptimization() { } optimized_submap_transforms_ = submap_data; connected_components_ = trajectory_connectivity_.ConnectedComponents(); - reverse_connected_components_.clear(); - for (size_t i = 0; i != connected_components_.size(); ++i) { - for (const int trajectory_id : connected_components_[i]) { - reverse_connected_components_.emplace(trajectory_id, i); - } - } TrimmingHandle trimming_handle(this); for (auto& trimmer : trimmers_) { diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index ffb2dd2..bc94b28 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -196,8 +196,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; - // Trajectory ID to connected component ID. - std::map reverse_connected_components_; // Data that are currently being shown. mapping::NestedVectorsById diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 8da99a8..3a680d4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -212,13 +212,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, initial_relative_pose.rotation()); } else { - const bool scan_and_submap_trajectories_connected = - reverse_connected_components_.count(node_id.trajectory_id) > 0 && - reverse_connected_components_.count(submap_id.trajectory_id) > 0 && - reverse_connected_components_.at(node_id.trajectory_id) == - reverse_connected_components_.at(submap_id.trajectory_id); - if (node_id.trajectory_id == submap_id.trajectory_id || - scan_and_submap_trajectories_connected) { + if (trajectory_connectivity_.TransitivelyConnected( + node_id.trajectory_id, submap_id.trajectory_id)) { constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, @@ -509,12 +504,6 @@ void SparsePoseGraph::RunOptimization() { } optimized_submap_transforms_ = optimization_problem_.submap_data(); connected_components_ = trajectory_connectivity_.ConnectedComponents(); - reverse_connected_components_.clear(); - for (size_t i = 0; i != connected_components_.size(); ++i) { - for (const int trajectory_id : connected_components_[i]) { - reverse_connected_components_.emplace(trajectory_id, i); - } - } TrimmingHandle trimming_handle(this); for (auto& trimmer : trimmers_) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 6cfdd16..ef81186 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -201,8 +201,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Connectivity structure of our trajectories by IDs. std::vector> connected_components_; - // Trajectory ID to connected component ID. - std::map reverse_connected_components_; // Data that are currently being shown. mapping::NestedVectorsById