From 24c2b499dd3200c2d73daa35afb96b6a5f0a878f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Mon, 11 Sep 2017 14:43:55 +0200 Subject: [PATCH] Introduce TrajectoryConnectivityState. (#513) * Introduce TrajectoryConnectivityState. This class will be used to track the connectivity state (including the last connection time) between pairs of trajectories. --- cartographer/mapping/connected_components.cc | 15 ++- cartographer/mapping/connected_components.h | 4 + .../mapping/trajectory_connectivity_state.cc | 74 +++++++++++++++ .../mapping/trajectory_connectivity_state.h | 80 ++++++++++++++++ .../trajectory_connectivity_state_test.cc | 91 +++++++++++++++++++ cartographer/mapping_2d/sparse_pose_graph.cc | 32 ++++--- cartographer/mapping_2d/sparse_pose_graph.h | 8 +- cartographer/mapping_3d/sparse_pose_graph.cc | 32 ++++--- cartographer/mapping_3d/sparse_pose_graph.h | 8 +- 9 files changed, 314 insertions(+), 30 deletions(-) create mode 100644 cartographer/mapping/trajectory_connectivity_state.cc create mode 100644 cartographer/mapping/trajectory_connectivity_state.h create mode 100644 cartographer/mapping/trajectory_connectivity_state_test.cc diff --git a/cartographer/mapping/connected_components.cc b/cartographer/mapping/connected_components.cc index 9787316..1761e3b 100644 --- a/cartographer/mapping/connected_components.cc +++ b/cartographer/mapping/connected_components.cc @@ -60,7 +60,7 @@ int ConnectedComponents::FindSet(const int trajectory_id) { } bool ConnectedComponents::TransitivelyConnected(const int trajectory_id_a, - const int trajectory_id_b) { + const int trajectory_id_b) { if (trajectory_id_a == trajectory_id_b) { return true; } @@ -91,8 +91,19 @@ std::vector> ConnectedComponents::Components() { return result; } +std::vector ConnectedComponents::GetComponent(const int trajectory_id) { + const int set_id = FindSet(trajectory_id); + std::vector trajectory_ids; + for (const auto& entry : forest_) { + if (FindSet(entry.first) == set_id) { + trajectory_ids.push_back(entry.first); + } + } + return trajectory_ids; +} + int ConnectedComponents::ConnectionCount(const int trajectory_id_a, - const int trajectory_id_b) { + const int trajectory_id_b) { common::MutexLocker locker(&lock_); const auto it = connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b)); diff --git a/cartographer/mapping/connected_components.h b/cartographer/mapping/connected_components.h index b0ed21d..dbf3e8a 100644 --- a/cartographer/mapping/connected_components.h +++ b/cartographer/mapping/connected_components.h @@ -63,6 +63,10 @@ class ConnectedComponents { // The trajectory IDs, grouped by connectivity. std::vector> Components() EXCLUDES(lock_); + // The list of trajectory IDs that belong to the same connected component as + // 'trajectory_id'. + std::vector GetComponent(int trajectory_id) EXCLUDES(lock_); + private: // Find the representative and compresses the path to it. int FindSet(int trajectory_id) REQUIRES(lock_); diff --git a/cartographer/mapping/trajectory_connectivity_state.cc b/cartographer/mapping/trajectory_connectivity_state.cc new file mode 100644 index 0000000..ee733af --- /dev/null +++ b/cartographer/mapping/trajectory_connectivity_state.cc @@ -0,0 +1,74 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/trajectory_connectivity_state.h" + +namespace cartographer { +namespace mapping { + +void TrajectoryConnectivityState::Add(const int trajectory_id) { + connected_components_.Add(trajectory_id); +} + +void TrajectoryConnectivityState::Connect(const int trajectory_id_a, + const int trajectory_id_b, + const common::Time time) { + if (TransitivelyConnected(trajectory_id_a, trajectory_id_b)) { + // The trajectories are transitively connected, i.e. they belong to the same + // connected component. In this case we only update the last connection time + // of those two trajectories. + auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); + if (last_connection_time_map_[sorted_pair] < time) { + last_connection_time_map_[sorted_pair] = time; + } + } else { + // The connection between these two trajectories is about to join to + // connected components. Here we update all bipartite trajectory pairs for + // the two connected components with the connection time. This is to quickly + // change to a more efficient loop closure search (by constraining the + // search window) when connected components are joined. + std::vector component_a = connected_components_.GetComponent(trajectory_id_a); + std::vector component_b = connected_components_.GetComponent(trajectory_id_b); + for (const auto id_a : component_a) { + for (const auto id_b : component_b) { + auto id_pair = std::minmax(id_a, id_b); + last_connection_time_map_[id_pair] = time; + } + } + } + connected_components_.Connect(trajectory_id_a, trajectory_id_b); +} + +bool TrajectoryConnectivityState::TransitivelyConnected( + const int trajectory_id_a, + const int trajectory_id_b) { + return connected_components_.TransitivelyConnected(trajectory_id_a, + trajectory_id_b); +} + +std::vector> TrajectoryConnectivityState::Components() { + return connected_components_.Components(); +} + +common::Time TrajectoryConnectivityState::LastConnectionTime( + const int trajectory_id_a, + const int trajectory_id_b) { + auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); + return last_connection_time_map_[sorted_pair]; +} + +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/trajectory_connectivity_state.h b/cartographer/mapping/trajectory_connectivity_state.h new file mode 100644 index 0000000..ab94963 --- /dev/null +++ b/cartographer/mapping/trajectory_connectivity_state.h @@ -0,0 +1,80 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_ +#define CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_ + +#include "cartographer/common/time.h" +#include "cartographer/mapping/connected_components.h" + +namespace cartographer { +namespace mapping { + +// A class that tracks the connectivity state between trajectories. Compared to +// ConnectedComponents it tracks additionally the last time that a global +// constraint connected to trajectories. +// +// This class is thread-compatible. +class TrajectoryConnectivityState { + public: + TrajectoryConnectivityState() {} + + TrajectoryConnectivityState(const TrajectoryConnectivityState&) = delete; + TrajectoryConnectivityState& operator=(const TrajectoryConnectivityState&) = + delete; + + // Add a trajectory which is initially connected to only itself. + void Add(int trajectory_id); + + // Connect two trajectories. If either trajectory is untracked, it will be + // tracked. This function is invariant to the order of its arguments. Repeated + // calls to Connect increment the connectivity count and update the last + // connected time. + void Connect(int trajectory_id_a, + int trajectory_id_b, + common::Time time); + + // Determines if two trajectories have been (transitively) connected. If + // 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); + + // The trajectory IDs, grouped by connectivity. + std::vector> Components(); + + // Return the last connection count between the two trajectories. If either of + // the trajectories is untracked or they have never been connected returns the + // beginning of time. + common::Time LastConnectionTime(int trajectory_id_a, int trajectory_id_b); + + private: + ConnectedComponents connected_components_; + + // Tracks the last time a direct connection between two trajectories has + // been added. The exception is when a connection between two trajectories + // connects two formerly unconnected connected components. In this case all + // bipartite trajectories entries for these components are updated with the + // new connection time. + std::map, common::Time> last_connection_time_map_; + +}; + +} // namespace mapping +} // namespace cartographer + +#endif // CARTOGRAPHER_MAPPING_TRAJECTORY_CONNECTIVITY_STATE_H_ diff --git a/cartographer/mapping/trajectory_connectivity_state_test.cc b/cartographer/mapping/trajectory_connectivity_state_test.cc new file mode 100644 index 0000000..517f01d --- /dev/null +++ b/cartographer/mapping/trajectory_connectivity_state_test.cc @@ -0,0 +1,91 @@ +/* + * Copyright 2017 The Cartographer Authors + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "cartographer/mapping/trajectory_connectivity_state.h" + +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { + +TEST(TrajectoryConnectivityStateTest, UnknownTrajectory) { + TrajectoryConnectivityState state; + state.Add(0); + + // Nothing is transitively connected to an unknown trajectory. + EXPECT_FALSE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time()); +} + +TEST(TrajectoryConnectivityStateTest, NotConnected) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + EXPECT_FALSE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::Time()); +} + +TEST(TrajectoryConnectivityStateTest, Connected) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + state.Connect(0, 1, common::FromUniversal(123456)); + EXPECT_TRUE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456)); +} + +TEST(TrajectoryConnectivityStateTest, UpdateConnectionTime) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + state.Connect(0, 1, common::FromUniversal(123456)); + state.Connect(0, 1, common::FromUniversal(234567)); + EXPECT_TRUE(state.TransitivelyConnected(0, 1)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567)); + + // Connections with an earlier connection time do not update the last + // connection time. + state.Connect(0, 1, common::FromUniversal(123456)); + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(234567)); +} + +TEST(TrajectoryConnectivityStateTest, JoinTwoComponents) { + TrajectoryConnectivityState state; + state.Add(0); + state.Add(1); + state.Add(2); + state.Add(3); + state.Connect(0, 1, common::FromUniversal(123456)); + state.Connect(2, 3, common::FromUniversal(123456)); + + // Connect the two disjoint connected components. + state.Connect(0, 2, common::FromUniversal(234567)); + EXPECT_TRUE(state.TransitivelyConnected(0, 2)); + EXPECT_TRUE(state.TransitivelyConnected(1, 3)); + + // All bipartite trajectory pairs between the two connected components should + // have the updated connection time. + EXPECT_EQ(state.LastConnectionTime(0, 2), common::FromUniversal(234567)); + EXPECT_EQ(state.LastConnectionTime(0, 3), common::FromUniversal(234567)); + EXPECT_EQ(state.LastConnectionTime(1, 3), common::FromUniversal(234567)); + + // A pair of trajectory IDs belonging to the same connected component should + // be unaffected. + EXPECT_EQ(state.LastConnectionTime(0, 1), common::FromUniversal(123456)); +} + +} // namespace cartographer +} // namespace mapping diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 06c9dde..225fcc7 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -104,7 +104,7 @@ void SparsePoseGraph::AddScan( trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; - connected_components_.Add(trajectory_id); + trajectory_connectivity_state_.Add(trajectory_id); // Test if the 'insertion_submap.back()' is one we never saw before. if (trajectory_id >= submap_data_.num_trajectories() || @@ -180,7 +180,7 @@ 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 { - if (connected_components_.TransitivelyConnected( + if (trajectory_connectivity_state_.TransitivelyConnected( node_id.trajectory_id, submap_id.trajectory_id)) { const transform::Rigid2d initial_relative_pose = optimization_problem_.submap_data() @@ -300,6 +300,22 @@ void SparsePoseGraph::ComputeConstraintsForScan( } } +void SparsePoseGraph::UpdateTrajectoryConnectivity( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + for (const Constraint& constraint : result) { + CHECK_EQ(constraint.tag, + mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); + mapping::NodeId last_submap_node_id = + *submap_data_.at(constraint.submap_id).node_ids.rbegin(); + common::Time time = + std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time, + trajectory_nodes_.at(last_submap_node_id).constant_data->time); + trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, + constraint.submap_id.trajectory_id, + time); + } +} + void SparsePoseGraph::HandleWorkQueue() { constraint_builder_.WhenDone( [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { @@ -308,15 +324,7 @@ void SparsePoseGraph::HandleWorkQueue() { constraints_.insert(constraints_.end(), result.begin(), result.end()); } RunOptimization(); - - // Update the trajectory connectivity structure with the new - // constraints. - for (const Constraint& constraint : result) { - CHECK_EQ(constraint.tag, - mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); - connected_components_.Connect(constraint.node_id.trajectory_id, - constraint.submap_id.trajectory_id); - } + UpdateTrajectoryConnectivity(result); common::MutexLocker locker(&mutex_); num_scans_since_last_loop_closure_ = 0; @@ -503,7 +511,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( } std::vector> SparsePoseGraph::GetConnectedTrajectories() { - return connected_components_.Components(); + return trajectory_connectivity_state_.Components(); } int SparsePoseGraph::num_submaps(const int trajectory_id) { diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index c265ace..0741fca 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -32,9 +32,9 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/connected_components.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_2d/submaps.h" @@ -153,6 +153,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // optimization being run at a time. void RunOptimization() EXCLUDES(mutex_); + // Updates the trajectory connectivity structure with the new constraints. + void UpdateTrajectoryConnectivity( + const sparse_pose_graph::ConstraintBuilder::Result& result); + // Computes the local to global frame transform based on the given optimized // 'submap_transforms'. transform::Rigid3d ComputeLocalToGlobalTransform( @@ -172,7 +176,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GUARDED_BY(mutex_); // How our various trajectories are related. - mapping::ConnectedComponents connected_components_; + mapping::TrajectoryConnectivityState trajectory_connectivity_state_; // We globally localize a fraction of the scans from each trajectory. std::unordered_map> diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index a2ef61b..ea2809c 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -103,7 +103,7 @@ void SparsePoseGraph::AddScan( trajectory_nodes_.Append( trajectory_id, mapping::TrajectoryNode{constant_data, optimized_pose}); ++num_trajectory_nodes_; - connected_components_.Add(trajectory_id); + trajectory_connectivity_state_.Add(trajectory_id); // Test if the 'insertion_submap.back()' is one we never saw before. if (trajectory_id >= submap_data_.num_trajectories() || @@ -213,7 +213,7 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, initial_relative_pose.rotation()); } else { - if (connected_components_.TransitivelyConnected( + if (trajectory_connectivity_state_.TransitivelyConnected( node_id.trajectory_id, submap_id.trajectory_id)) { constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, @@ -320,6 +320,22 @@ void SparsePoseGraph::ComputeConstraintsForScan( } } +void SparsePoseGraph::UpdateTrajectoryConnectivity( + const sparse_pose_graph::ConstraintBuilder::Result& result) { + for (const Constraint& constraint : result) { + CHECK_EQ(constraint.tag, + mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); + const mapping::NodeId last_submap_node_id = + *submap_data_.at(constraint.submap_id).node_ids.rbegin(); + const common::Time time = + std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time, + trajectory_nodes_.at(last_submap_node_id).constant_data->time); + trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, + constraint.submap_id.trajectory_id, + time); + } +} + void SparsePoseGraph::HandleWorkQueue() { constraint_builder_.WhenDone( [this](const sparse_pose_graph::ConstraintBuilder::Result& result) { @@ -328,15 +344,7 @@ void SparsePoseGraph::HandleWorkQueue() { constraints_.insert(constraints_.end(), result.begin(), result.end()); } RunOptimization(); - - // Update the trajectory connectivity structure with the new - // constraints. - for (const Constraint& constraint : result) { - CHECK_EQ(constraint.tag, - mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); - connected_components_.Connect(constraint.node_id.trajectory_id, - constraint.submap_id.trajectory_id); - } + UpdateTrajectoryConnectivity(result); common::MutexLocker locker(&mutex_); num_scans_since_last_loop_closure_ = 0; @@ -542,7 +550,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( } std::vector> SparsePoseGraph::GetConnectedTrajectories() { - return connected_components_.Components(); + return trajectory_connectivity_state_.Components(); } int SparsePoseGraph::num_submaps(const int trajectory_id) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 3009e4e..bc47807 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -32,9 +32,9 @@ #include "cartographer/common/mutex.h" #include "cartographer/common/thread_pool.h" #include "cartographer/common/time.h" -#include "cartographer/mapping/connected_components.h" #include "cartographer/mapping/pose_graph_trimmer.h" #include "cartographer/mapping/sparse_pose_graph.h" +#include "cartographer/mapping/trajectory_connectivity_state.h" #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h" #include "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h" #include "cartographer/mapping_3d/submaps.h" @@ -168,6 +168,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // poses. void LogResidualHistograms() REQUIRES(mutex_); + // Updates the trajectory connectivity structure with the new constraints. + void UpdateTrajectoryConnectivity( + const sparse_pose_graph::ConstraintBuilder::Result& result); + const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; @@ -177,7 +181,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { GUARDED_BY(mutex_); // How our various trajectories are related. - mapping::ConnectedComponents connected_components_; + mapping::TrajectoryConnectivityState trajectory_connectivity_state_; // We globally localize a fraction of the scans from each trajectory. std::unordered_map>