From e6a6bab351ff389806d2812ba10af5eab979b68d Mon Sep 17 00:00:00 2001 From: Holger Rapp Date: Thu, 11 May 2017 13:36:12 +0200 Subject: [PATCH] Make TrajectoryConnectivity use trajectory IDs. (#275) Related to #256. --- cartographer/mapping/map_builder.cc | 3 +- cartographer/mapping/sparse_pose_graph.h | 5 +- .../mapping/trajectory_connectivity.cc | 84 +++++++--------- .../mapping/trajectory_connectivity.h | 39 +++----- .../mapping/trajectory_connectivity_test.cc | 97 +++++++------------ cartographer/mapping_2d/sparse_pose_graph.cc | 43 ++++---- cartographer/mapping_2d/sparse_pose_graph.h | 14 +-- .../sparse_pose_graph/constraint_builder.cc | 18 ++-- .../sparse_pose_graph/constraint_builder.h | 15 ++- cartographer/mapping_3d/sparse_pose_graph.cc | 44 +++++---- cartographer/mapping_3d/sparse_pose_graph.h | 14 +-- .../sparse_pose_graph/constraint_builder.cc | 18 ++-- .../sparse_pose_graph/constraint_builder.h | 13 +-- 13 files changed, 172 insertions(+), 235 deletions(-) diff --git a/cartographer/mapping/map_builder.cc b/cartographer/mapping/map_builder.cc index f26bc32..5e1207f 100644 --- a/cartographer/mapping/map_builder.cc +++ b/cartographer/mapping/map_builder.cc @@ -113,8 +113,7 @@ int MapBuilder::GetTrajectoryId(const Submaps* const trajectory) const { } proto::TrajectoryConnectivity MapBuilder::GetTrajectoryConnectivity() { - return ToProto(sparse_pose_graph_->GetConnectedTrajectories(), - trajectory_ids_); + return ToProto(sparse_pose_graph_->GetConnectedTrajectories()); } string MapBuilder::SubmapToProto(const int trajectory_id, diff --git a/cartographer/mapping/sparse_pose_graph.h b/cartographer/mapping/sparse_pose_graph.h index abe2441..352e5ce 100644 --- a/cartographer/mapping/sparse_pose_graph.h +++ b/cartographer/mapping/sparse_pose_graph.h @@ -58,7 +58,7 @@ class SparsePoseGraph { Eigen::Matrix sqrt_Lambda_ij; }; - mapping::SubmapId submap_id; // 'i' in the paper. + mapping::SubmapId submap_id; // 'i' in the paper. // Scan index. int j; @@ -82,8 +82,7 @@ class SparsePoseGraph { virtual void RunFinalOptimization() = 0; // Get the current trajectory clusters. - virtual std::vector> - GetConnectedTrajectories() = 0; + virtual std::vector> GetConnectedTrajectories() = 0; // Returns the current optimized transforms for the given 'trajectory'. virtual std::vector GetSubmapTransforms( diff --git a/cartographer/mapping/trajectory_connectivity.cc b/cartographer/mapping/trajectory_connectivity.cc index 24a0144..1ae65e3 100644 --- a/cartographer/mapping/trajectory_connectivity.cc +++ b/cartographer/mapping/trajectory_connectivity.cc @@ -28,35 +28,30 @@ namespace mapping { TrajectoryConnectivity::TrajectoryConnectivity() : lock_(), forest_(), connection_map_() {} -void TrajectoryConnectivity::Add(const Submaps* trajectory) { - CHECK(trajectory != nullptr); +void TrajectoryConnectivity::Add(const int trajectory_id) { common::MutexLocker locker(&lock_); - forest_.emplace(trajectory, trajectory); + forest_.emplace(trajectory_id, trajectory_id); } -void TrajectoryConnectivity::Connect(const Submaps* trajectory_a, - const Submaps* trajectory_b) { - CHECK(trajectory_a != nullptr); - CHECK(trajectory_b != nullptr); +void TrajectoryConnectivity::Connect(const int trajectory_id_a, + const int trajectory_id_b) { common::MutexLocker locker(&lock_); - Union(trajectory_a, trajectory_b); - auto sorted_pair = - std::minmax(trajectory_a, trajectory_b, std::less()); + Union(trajectory_id_a, trajectory_id_b); + auto sorted_pair = std::minmax(trajectory_id_a, trajectory_id_b); ++connection_map_[sorted_pair]; } -void TrajectoryConnectivity::Union(const Submaps* const trajectory_a, - const Submaps* const trajectory_b) { - forest_.emplace(trajectory_a, trajectory_a); - forest_.emplace(trajectory_b, trajectory_b); - const Submaps* const representative_a = FindSet(trajectory_a); - const Submaps* const representative_b = FindSet(trajectory_b); +void TrajectoryConnectivity::Union(const int trajectory_id_a, + const int trajectory_id_b) { + forest_.emplace(trajectory_id_a, trajectory_id_a); + forest_.emplace(trajectory_id_b, trajectory_id_b); + const int representative_a = FindSet(trajectory_id_a); + const int representative_b = FindSet(trajectory_id_b); forest_[representative_a] = representative_b; } -const Submaps* TrajectoryConnectivity::FindSet( - const Submaps* const trajectory) { - auto it = forest_.find(trajectory); +int TrajectoryConnectivity::FindSet(const int trajectory_id) { + auto it = forest_.find(trajectory_id); CHECK(it != forest_.end()); if (it->first != it->second) { it->second = FindSet(it->second); @@ -64,28 +59,27 @@ const Submaps* TrajectoryConnectivity::FindSet( return it->second; } -bool TrajectoryConnectivity::TransitivelyConnected( - const Submaps* trajectory_a, const Submaps* trajectory_b) { - CHECK(trajectory_a != nullptr); - CHECK(trajectory_b != nullptr); +bool TrajectoryConnectivity::TransitivelyConnected(const int trajectory_id_a, + const int trajectory_id_b) { common::MutexLocker locker(&lock_); - if (forest_.count(trajectory_a) == 0 || forest_.count(trajectory_b) == 0) { + if (forest_.count(trajectory_id_a) == 0 || + forest_.count(trajectory_id_b) == 0) { return false; } - return FindSet(trajectory_a) == FindSet(trajectory_b); + return FindSet(trajectory_id_a) == FindSet(trajectory_id_b); } -std::vector> -TrajectoryConnectivity::ConnectedComponents() { +std::vector> TrajectoryConnectivity::ConnectedComponents() { // Map from cluster exemplar -> growing cluster. - std::unordered_map> map; + std::unordered_map> map; common::MutexLocker locker(&lock_); - for (const auto& trajectory_entry : forest_) { - map[FindSet(trajectory_entry.first)].push_back(trajectory_entry.first); + for (const auto& trajectory_id_entry : forest_) { + map[FindSet(trajectory_id_entry.first)].push_back( + trajectory_id_entry.first); } - std::vector> result; + std::vector> result; result.reserve(map.size()); for (auto& pair : map) { result.emplace_back(std::move(pair.second)); @@ -93,33 +87,23 @@ TrajectoryConnectivity::ConnectedComponents() { return result; } -int TrajectoryConnectivity::ConnectionCount(const Submaps* trajectory_a, - const Submaps* trajectory_b) { - CHECK(trajectory_a != nullptr); - CHECK(trajectory_b != nullptr); +int TrajectoryConnectivity::ConnectionCount(const int trajectory_id_a, + const int trajectory_id_b) { common::MutexLocker locker(&lock_); - const auto it = connection_map_.find( - std::minmax(trajectory_a, trajectory_b, std::less())); + const auto it = + connection_map_.find(std::minmax(trajectory_id_a, trajectory_id_b)); return it != connection_map_.end() ? it->second : 0; } proto::TrajectoryConnectivity ToProto( - std::vector> connected_components, - std::unordered_map trajectory_indices) { + std::vector> connected_components) { proto::TrajectoryConnectivity proto; - std::vector> connected_components_by_indices; for (const auto& connected_component : connected_components) { - connected_components_by_indices.emplace_back(); - for (const mapping::Submaps* trajectory : connected_component) { - connected_components_by_indices.back().push_back( - trajectory_indices.at(trajectory)); - } - std::sort(connected_components_by_indices.back().begin(), - connected_components_by_indices.back().end()); + std::sort(connected_components.back().begin(), + connected_components.back().end()); } - std::sort(connected_components_by_indices.begin(), - connected_components_by_indices.end()); - for (const auto& connected_component : connected_components_by_indices) { + std::sort(connected_components.begin(), connected_components.end()); + for (const auto& connected_component : connected_components) { auto* proto_connected_component = proto.add_connected_component(); for (const int trajectory_id : connected_component) { proto_connected_component->add_trajectory_id(trajectory_id); diff --git a/cartographer/mapping/trajectory_connectivity.h b/cartographer/mapping/trajectory_connectivity.h index dbc466a..4e0b8cf 100644 --- a/cartographer/mapping/trajectory_connectivity.h +++ b/cartographer/mapping/trajectory_connectivity.h @@ -41,50 +41,43 @@ class TrajectoryConnectivity { TrajectoryConnectivity& operator=(const TrajectoryConnectivity&) = delete; // Add a trajectory which is initially connected to nothing. - void Add(const Submaps* trajectory) EXCLUDES(lock_); + void Add(int trajectory_id) EXCLUDES(lock_); // 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. - void Connect(const Submaps* trajectory_a, const Submaps* trajectory_b) - EXCLUDES(lock_); + 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. - bool TransitivelyConnected(const Submaps* trajectory_a, - const Submaps* trajectory_b) EXCLUDES(lock_); - - // Return the number of _direct_ connections between trajectory_a and - // trajectory_b. If either trajectory is not being tracked, returns 0. This - // function is invariant to the order of its arguments. - int ConnectionCount(const Submaps* trajectory_a, const Submaps* trajectory_b) + bool TransitivelyConnected(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); - // The trajectories, grouped by connectivity. - std::vector> ConnectedComponents() - EXCLUDES(lock_); + // Return the number of _direct_ connections between 'trajectory_id_a' and + // 'trajectory_id_b'. If either trajectory is not being tracked, returns 0. + // This function is invariant to the order of its arguments. + int ConnectionCount(int trajectory_id_a, int trajectory_id_b) EXCLUDES(lock_); + + // The trajectory IDs, grouped by connectivity. + std::vector> ConnectedComponents() EXCLUDES(lock_); private: // Find the representative and compresses the path to it. - const Submaps* FindSet(const Submaps* trajectory) REQUIRES(lock_); - void Union(const Submaps* trajectory_a, const Submaps* trajectory_b) - REQUIRES(lock_); + int FindSet(int trajectory_id) REQUIRES(lock_); + void Union(int trajectory_id_a, int trajectory_id_b) REQUIRES(lock_); common::Mutex lock_; // Tracks transitive connectivity using a disjoint set forest, i.e. each // entry points towards the representative for the given trajectory. - std::map forest_ GUARDED_BY(lock_); + std::map forest_ GUARDED_BY(lock_); // Tracks the number of direct connections between a pair of trajectories. - std::map, int> connection_map_ - GUARDED_BY(lock_); + std::map, int> connection_map_ GUARDED_BY(lock_); }; -// Returns a proto encoding connected components according to -// 'trajectory_indices'. +// Returns a proto encoding connected components. proto::TrajectoryConnectivity ToProto( - std::vector> connected_components, - std::unordered_map trajectory_indices); + std::vector> connected_components); // Returns the connected component containing 'trajectory_index'. proto::TrajectoryConnectivity::ConnectedComponent FindConnectedComponent( diff --git a/cartographer/mapping/trajectory_connectivity_test.cc b/cartographer/mapping/trajectory_connectivity_test.cc index b640e78..fe4dd62 100644 --- a/cartographer/mapping/trajectory_connectivity_test.cc +++ b/cartographer/mapping/trajectory_connectivity_test.cc @@ -20,88 +20,60 @@ #include #include -#include "cartographer/common/lua_parameter_dictionary_test_helpers.h" -#include "cartographer/mapping_2d/submaps.h" #include "gtest/gtest.h" namespace cartographer { namespace mapping { namespace { -class TrajectoryConnectivityTest : public ::testing::Test { - protected: - TrajectoryConnectivityTest() { - for (int i = 0; i < 10; ++i) { - auto parameter_dictionary = common::MakeDictionary(R"text( - return { - resolution = 0.05, - half_length = 10., - num_range_data = 10, - output_debug_images = false, - range_data_inserter = { - insert_free_space = true, - hit_probability = 0.53, - miss_probability = 0.495, - }, - })text"); - trajectories_.emplace_back(new mapping_2d::Submaps( - mapping_2d::CreateSubmapsOptions(parameter_dictionary.get()))); - } - } +constexpr int kNumTrajectories = 10; - // Helper function to avoid .get() noise. - const Submaps* trajectory(int index) { return trajectories_.at(index).get(); } +TEST(TrajectoryConnectivityTest, TransitivelyConnected) { + TrajectoryConnectivity trajectory_connectivity; - TrajectoryConnectivity trajectory_connectivity_; - std::vector> trajectories_; -}; - -TEST_F(TrajectoryConnectivityTest, TransitivelyConnected) { // Make sure nothing's connected until we connect some things. - for (auto& trajectory_a : trajectories_) { - for (auto& trajectory_b : trajectories_) { - EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected( - trajectory_a.get(), trajectory_b.get())); + for (int trajectory_a; trajectory_a < kNumTrajectories; ++trajectory_a) { + for (int trajectory_b; trajectory_b < kNumTrajectories; ++trajectory_b) { + EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(trajectory_a, + trajectory_b)); } } // Connect some stuff up. - trajectory_connectivity_.Connect(trajectory(0), trajectory(1)); - EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(0), - trajectory(1))); - trajectory_connectivity_.Connect(trajectory(8), trajectory(9)); - EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected(trajectory(8), - trajectory(9))); - EXPECT_FALSE(trajectory_connectivity_.TransitivelyConnected(trajectory(0), - trajectory(9))); + trajectory_connectivity.Connect(0, 1); + EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(0, 1)); + trajectory_connectivity.Connect(8, 9); + EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(8, 9)); + EXPECT_FALSE(trajectory_connectivity.TransitivelyConnected(0, 9)); - trajectory_connectivity_.Connect(trajectory(1), trajectory(8)); + trajectory_connectivity.Connect(1, 8); for (int i : {0, 1}) { for (int j : {8, 9}) { - EXPECT_TRUE(trajectory_connectivity_.TransitivelyConnected( - trajectory(i), trajectory(j))); + EXPECT_TRUE(trajectory_connectivity.TransitivelyConnected(i, j)); } } } -TEST_F(TrajectoryConnectivityTest, EmptyConnectedComponents) { - auto connections = trajectory_connectivity_.ConnectedComponents(); +TEST(TrajectoryConnectivityTest, EmptyConnectedComponents) { + TrajectoryConnectivity trajectory_connectivity; + auto connections = trajectory_connectivity.ConnectedComponents(); EXPECT_EQ(0, connections.size()); } -TEST_F(TrajectoryConnectivityTest, ConnectedComponents) { +TEST(TrajectoryConnectivityTest, ConnectedComponents) { + TrajectoryConnectivity trajectory_connectivity; for (int i = 0; i <= 4; ++i) { - trajectory_connectivity_.Connect(trajectory(0), trajectory(i)); + trajectory_connectivity.Connect(0, i); } for (int i = 5; i <= 9; ++i) { - trajectory_connectivity_.Connect(trajectory(5), trajectory(i)); + trajectory_connectivity.Connect(5, i); } - auto connections = trajectory_connectivity_.ConnectedComponents(); + auto connections = trajectory_connectivity.ConnectedComponents(); ASSERT_EQ(2, connections.size()); // The clustering is arbitrary; we need to figure out which one is which. - const std::vector* zero_cluster = nullptr; - const std::vector* five_cluster = nullptr; - if (std::find(connections[0].begin(), connections[0].end(), trajectory(0)) != + const std::vector* zero_cluster = nullptr; + const std::vector* five_cluster = nullptr; + if (std::find(connections[0].begin(), connections[0].end(), 0) != connections[0].end()) { zero_cluster = &connections[0]; five_cluster = &connections[1]; @@ -111,22 +83,21 @@ TEST_F(TrajectoryConnectivityTest, ConnectedComponents) { } for (int i = 0; i <= 9; ++i) { EXPECT_EQ(i <= 4, std::find(zero_cluster->begin(), zero_cluster->end(), - trajectory(i)) != zero_cluster->end()); - EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), - trajectory(i)) != five_cluster->end()); + i) != zero_cluster->end()); + EXPECT_EQ(i > 4, std::find(five_cluster->begin(), five_cluster->end(), i) != + five_cluster->end()); } } -TEST_F(TrajectoryConnectivityTest, ConnectionCount) { - for (int i = 0; i < 10; ++i) { - trajectory_connectivity_.Connect(trajectory(0), trajectory(1)); +TEST(TrajectoryConnectivityTest, ConnectionCount) { + TrajectoryConnectivity trajectory_connectivity; + for (int i = 0; i < kNumTrajectories; ++i) { + trajectory_connectivity.Connect(0, 1); // Permute the arguments to check invariance. - EXPECT_EQ(i + 1, trajectory_connectivity_.ConnectionCount(trajectory(1), - trajectory(0))); + EXPECT_EQ(i + 1, trajectory_connectivity.ConnectionCount(1, 0)); } for (int i = 1; i < 9; ++i) { - EXPECT_EQ(0, trajectory_connectivity_.ConnectionCount(trajectory(i), - trajectory(i + 1))); + EXPECT_EQ(0, trajectory_connectivity.ConnectionCount(i, i + 1)); } } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index c731cb1..6211d8e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -99,6 +99,7 @@ void SparsePoseGraph::AddScan( common::MutexLocker locker(&mutex_); trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); + const int trajectory_id = trajectory_ids_.at(trajectory); const int j = trajectory_nodes_.size(); CHECK_LT(j, std::numeric_limits::max()); @@ -109,16 +110,15 @@ void SparsePoseGraph::AddScan( trajectory_nodes_.push_back(mapping::TrajectoryNode{ &constant_node_data_.back(), optimized_pose, }); - trajectory_connectivity_.Add(trajectory); + trajectory_connectivity_.Add(trajectory_id); if (submap_indices_.count(insertion_submaps.back()) == 0) { submap_indices_.emplace(insertion_submaps.back(), static_cast(submap_indices_.size())); submap_states_.emplace_back(); submap_states_.back().submap = insertion_submaps.back(); - submap_states_.back().trajectory = trajectory; submap_states_.back().id = mapping::SubmapId{ - trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]}; + trajectory_id, num_submaps_in_trajectory_[trajectory]}; ++num_submaps_in_trajectory_[trajectory]; CHECK_EQ(submap_states_.size(), submap_indices_.size()); } @@ -172,23 +172,24 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, const mapping::Submaps* const scan_trajectory = trajectory_nodes_[scan_index].constant_data->trajectory; - const mapping::Submaps* const submap_trajectory = - submap_states_[submap_index].trajectory; + const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory); + const int submap_trajectory_id = + submap_states_[submap_index].id.trajectory_id; // Only globally match against submaps not in this trajectory. - if (scan_trajectory != submap_trajectory && + if (scan_trajectory_id != submap_trajectory_id && global_localization_samplers_[scan_trajectory]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_[submap_index].submap, scan_index, - scan_trajectory, submap_trajectory, &trajectory_connectivity_, + scan_trajectory_id, &trajectory_connectivity_, &trajectory_nodes_[scan_index].constant_data->range_data_2d.returns); } else { const bool scan_and_submap_trajectories_connected = - reverse_connected_components_.count(scan_trajectory) > 0 && - reverse_connected_components_.count(submap_trajectory) > 0 && - reverse_connected_components_.at(scan_trajectory) == - reverse_connected_components_.at(submap_trajectory); - if (scan_trajectory == submap_trajectory || + reverse_connected_components_.count(scan_trajectory_id) > 0 && + reverse_connected_components_.count(submap_trajectory_id) > 0 && + reverse_connected_components_.at(scan_trajectory_id) == + reverse_connected_components_.at(submap_trajectory_id); + if (scan_trajectory_id == submap_trajectory_id || scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( submap_id, submap_states_[submap_index].submap, scan_index, @@ -386,8 +387,8 @@ void SparsePoseGraph::RunOptimization() { connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { - for (const auto* trajectory : connected_components_[i]) { - reverse_connected_components_.emplace(trajectory, i); + for (const int trajectory_id : connected_components_[i]) { + reverse_connected_components_.emplace(trajectory_id, i); } } } @@ -417,8 +418,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( .inverse(); } -std::vector> -SparsePoseGraph::GetConnectedTrajectories() { +std::vector> SparsePoseGraph::GetConnectedTrajectories() { common::MutexLocker locker(&mutex_); return connected_components_; } @@ -433,17 +433,20 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, const mapping::Submaps* const trajectory) const { - std::vector result; + if (trajectory_ids_.count(trajectory) == 0) { + return {transform::Rigid3d::Identity()}; + } + const int trajectory_id = trajectory_ids_.at(trajectory); size_t flat_index = 0; size_t flat_index_of_result_back = -1; // Submaps for which we have optimized poses. + std::vector result; for (; flat_index != submap_states_.size(); ++flat_index) { const auto& state = submap_states_[flat_index]; - if (state.trajectory != trajectory) { + if (state.id.trajectory_id != trajectory_id) { continue; } - const int trajectory_id = trajectory_ids_.at(trajectory); if (static_cast(trajectory_id) >= submap_transforms.size() || result.size() >= submap_transforms.at(trajectory_id).size()) { break; @@ -456,7 +459,7 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( // Extrapolate to the remaining submaps. for (; flat_index != submap_states_.size(); ++flat_index) { const auto& state = submap_states_[flat_index]; - if (state.trajectory != trajectory) { + if (state.id.trajectory_id != trajectory_id) { continue; } if (result.empty()) { diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 342dffb..549767c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -83,8 +83,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; - std::vector> GetConnectedTrajectories() - override; + std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) @@ -112,9 +111,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // which are finished. bool finished = false; - // The trajectory to which this SubmapState belongs. - const mapping::Submaps* trajectory = nullptr; - mapping::SubmapId id; }; @@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::map num_submaps_in_trajectory_ GUARDED_BY(mutex_); - // Connectivity structure of our trajectories. - std::vector> connected_components_; - // Trajectory to connected component ID. - std::map reverse_connected_components_; + // 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. // diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc index f03fa2a..462085c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc @@ -78,8 +78,7 @@ void ConstraintBuilder::MaybeAddConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, scan_index, - nullptr, /* scan_trajectory */ - nullptr, /* submap_trajectory */ + -1, /* scan_trajectory_id */ false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ point_cloud, initial_relative_pose, constraint); @@ -90,8 +89,7 @@ void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, - const int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, + const int scan_index, const int scan_trajectory_id, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud) { common::MutexLocker locker(&mutex_); @@ -102,8 +100,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( const int current_computation = current_computation_; ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap->finished_probability_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, scan_index, submap_trajectory, - scan_trajectory, true, /* match_full_submap */ + ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id, + true, /* match_full_submap */ trajectory_connectivity, point_cloud, transform::Rigid2d::Identity(), constraint); FinishComputation(current_computation); @@ -168,8 +166,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* const submap, - const int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, bool match_full_submap, + const int scan_index, const int scan_trajectory_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* const point_cloud, const transform::Rigid2d& initial_relative_pose, @@ -194,7 +191,10 @@ void ConstraintBuilder::ComputeConstraint( filtered_point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); - trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); + CHECK_GE(scan_trajectory_id, 0); + CHECK_GE(submap_id.trajectory_id, 0); + trajectory_connectivity->Connect(scan_trajectory_id, + submap_id.trajectory_id); } else { return; } diff --git a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h index fe7373a..1a8c64d 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h @@ -35,7 +35,7 @@ #include "cartographer/mapping/trajectory_connectivity.h" #include "cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h" -#include "cartographer/mapping_2d/submaps.h" + #include "cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h" #include "cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h" #include "cartographer/sensor/point_cloud.h" @@ -86,16 +86,14 @@ class ConstraintBuilder { // 'submap_id' and the 'point_cloud' for 'scan_index'. This performs // full-submap matching. // - // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and - // the 'submap' should be from 'submap_trajectory'. The - // 'trajectory_connectivity' is updated if the full-submap match succeeds. + // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'. + // The 'trajectory_connectivity' is updated if the full-submap match succeeds. // // The pointees of 'submap' and 'point_cloud' must stay valid until all // computations are finished. void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* submap, - int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, + int scan_index, int scan_trajectory_id, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* point_cloud); @@ -134,13 +132,12 @@ class ConstraintBuilder { // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'point_cloud' do not change anymore. // If 'match_full_submap' is true, and global localization succeeds, will - // connect 'scan_trajectory' and 'submap_trajectory' in + // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const mapping::Submap* submap, - int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, bool match_full_submap, + int scan_index, int scan_trajectory_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::PointCloud* point_cloud, const transform::Rigid2d& initial_relative_pose, diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index b8eebfb..4f69078 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -97,6 +97,7 @@ void SparsePoseGraph::AddScan( common::MutexLocker locker(&mutex_); trajectory_ids_.emplace(trajectory, trajectory_ids_.size()); + const int trajectory_id = trajectory_ids_.at(trajectory); const int j = trajectory_nodes_.size(); CHECK_LT(j, std::numeric_limits::max()); @@ -106,16 +107,15 @@ void SparsePoseGraph::AddScan( transform::Rigid3d::Identity()}); trajectory_nodes_.push_back( mapping::TrajectoryNode{&constant_node_data_.back(), optimized_pose}); - trajectory_connectivity_.Add(trajectory); + trajectory_connectivity_.Add(trajectory_id); if (submap_indices_.count(insertion_submaps.back()) == 0) { submap_indices_.emplace(insertion_submaps.back(), static_cast(submap_indices_.size())); submap_states_.emplace_back(); submap_states_.back().submap = insertion_submaps.back(); - submap_states_.back().trajectory = trajectory; submap_states_.back().id = mapping::SubmapId{ - trajectory_ids_.at(trajectory), num_submaps_in_trajectory_[trajectory]}; + trajectory_id, num_submaps_in_trajectory_[trajectory]}; ++num_submaps_in_trajectory_[trajectory]; CHECK_EQ(submap_states_.size(), submap_indices_.size()); } @@ -172,23 +172,23 @@ void SparsePoseGraph::ComputeConstraint(const int scan_index, const mapping::Submaps* const scan_trajectory = trajectory_nodes_[scan_index].constant_data->trajectory; - const mapping::Submaps* const submap_trajectory = - submap_states_[submap_index].trajectory; + const int scan_trajectory_id = trajectory_ids_.at(scan_trajectory); + const int submap_trajectory_id = + submap_states_[submap_index].id.trajectory_id; // Only globally match against submaps not in this trajectory. - if (scan_trajectory != submap_trajectory && + if (scan_trajectory_id != submap_trajectory_id && global_localization_samplers_[scan_trajectory]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_states_[submap_index].submap, scan_index, - scan_trajectory, submap_trajectory, &trajectory_connectivity_, - trajectory_nodes_); + scan_trajectory_id, &trajectory_connectivity_, trajectory_nodes_); } else { const bool scan_and_submap_trajectories_connected = - reverse_connected_components_.count(scan_trajectory) > 0 && - reverse_connected_components_.count(submap_trajectory) > 0 && - reverse_connected_components_.at(scan_trajectory) == - reverse_connected_components_.at(submap_trajectory); - if (scan_trajectory == submap_trajectory || + reverse_connected_components_.count(scan_trajectory_id) > 0 && + reverse_connected_components_.count(submap_trajectory_id) > 0 && + reverse_connected_components_.at(scan_trajectory_id) == + reverse_connected_components_.at(submap_trajectory_id); + if (scan_trajectory_id == submap_trajectory_id || scan_and_submap_trajectories_connected) { constraint_builder_.MaybeAddConstraint( submap_id, submap_states_[submap_index].submap, scan_index, @@ -379,8 +379,8 @@ void SparsePoseGraph::RunOptimization() { connected_components_ = trajectory_connectivity_.ConnectedComponents(); reverse_connected_components_.clear(); for (size_t i = 0; i != connected_components_.size(); ++i) { - for (const auto* trajectory : connected_components_[i]) { - reverse_connected_components_.emplace(trajectory, i); + for (const int trajectory_id : connected_components_[i]) { + reverse_connected_components_.emplace(trajectory_id, i); } } } @@ -410,8 +410,7 @@ transform::Rigid3d SparsePoseGraph::GetLocalToGlobalTransform( .inverse(); } -std::vector> -SparsePoseGraph::GetConnectedTrajectories() { +std::vector> SparsePoseGraph::GetConnectedTrajectories() { common::MutexLocker locker(&mutex_); return connected_components_; } @@ -426,17 +425,20 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( const std::vector>& submap_transforms, const mapping::Submaps* const trajectory) const { - std::vector result; + if (trajectory_ids_.count(trajectory) == 0) { + return {transform::Rigid3d::Identity()}; + } + const int trajectory_id = trajectory_ids_.at(trajectory); size_t flat_index = 0; size_t flat_index_of_result_back = -1; // Submaps for which we have optimized poses. + std::vector result; for (; flat_index != submap_states_.size(); ++flat_index) { const auto& state = submap_states_[flat_index]; - if (state.trajectory != trajectory) { + if (state.id.trajectory_id != trajectory_id) { continue; } - const int trajectory_id = trajectory_ids_.at(trajectory); if (static_cast(trajectory_id) >= submap_transforms.size() || result.size() >= submap_transforms.at(trajectory_id).size()) { break; @@ -449,7 +451,7 @@ std::vector SparsePoseGraph::ExtrapolateSubmapTransforms( // Extrapolate to the remaining submaps. for (; flat_index != submap_states_.size(); ++flat_index) { const auto& state = submap_states_[flat_index]; - if (state.trajectory != trajectory) { + if (state.id.trajectory_id != trajectory_id) { continue; } if (result.empty()) { diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 918e2cb..3bddd63 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -85,8 +85,7 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { const Eigen::Vector3d& angular_velocity); void RunFinalOptimization() override; - std::vector> GetConnectedTrajectories() - override; + std::vector> GetConnectedTrajectories() override; std::vector GetSubmapTransforms( const mapping::Submaps* trajectory) EXCLUDES(mutex_) override; transform::Rigid3d GetLocalToGlobalTransform(const mapping::Submaps* submaps) @@ -114,9 +113,6 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // which are finished. bool finished = false; - // The trajectory to which this SubmapState belongs. - const Submaps* trajectory = nullptr; - mapping::SubmapId id; }; @@ -205,10 +201,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::map num_submaps_in_trajectory_ GUARDED_BY(mutex_); - // Connectivity structure of our trajectories. - std::vector> connected_components_; - // Trajectory to connected component ID. - std::map reverse_connected_components_; + // 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. // diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc index 36206ef..84cb65f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc @@ -100,8 +100,7 @@ void ConstraintBuilder::MaybeAddConstraint( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { ComputeConstraint(submap_id, submap, scan_index, - nullptr, /* scan_trajectory */ - nullptr, /* submap_trajectory */ + -1, /* scan_trajectory_id */ false, /* match_full_submap */ nullptr, /* trajectory_connectivity */ point_cloud, initial_relative_pose, constraint); @@ -112,8 +111,7 @@ void ConstraintBuilder::MaybeAddConstraint( void ConstraintBuilder::MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, - const int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, + const int scan_index, const int scan_trajectory_id, mapping::TrajectoryConnectivity* trajectory_connectivity, const std::vector& trajectory_nodes) { const auto submap_nodes = ComputeSubmapNodes( @@ -129,8 +127,8 @@ void ConstraintBuilder::MaybeAddGlobalConstraint( ScheduleSubmapScanMatcherConstructionAndQueueWorkItem( submap_id, submap_nodes, &submap->high_resolution_hybrid_grid, [=]() EXCLUDES(mutex_) { - ComputeConstraint(submap_id, submap, scan_index, submap_trajectory, - scan_trajectory, true, /* match_full_submap */ + ComputeConstraint(submap_id, submap, scan_index, scan_trajectory_id, + true, /* match_full_submap */ trajectory_connectivity, point_cloud, transform::Rigid3d::Identity(), constraint); FinishComputation(current_computation); @@ -200,8 +198,7 @@ ConstraintBuilder::GetSubmapScanMatcher(const mapping::SubmapId& submap_id) { void ConstraintBuilder::ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* const submap, - const int scan_index, const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, bool match_full_submap, + const int scan_index, const int scan_trajectory_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* const compressed_point_cloud, const transform::Rigid3d& initial_relative_pose, @@ -227,7 +224,10 @@ void ConstraintBuilder::ComputeConstraint( initial_pose.rotation(), filtered_point_cloud, point_cloud, options_.global_localization_min_score(), &score, &pose_estimate)) { CHECK_GT(score, options_.global_localization_min_score()); - trajectory_connectivity->Connect(scan_trajectory, submap_trajectory); + CHECK_GE(scan_trajectory_id, 0); + CHECK_GE(submap_id.trajectory_id, 0); + trajectory_connectivity->Connect(scan_trajectory_id, + submap_id.trajectory_id); } else { return; } diff --git a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h index 213c366..742aba4 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h +++ b/cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h @@ -82,16 +82,14 @@ class ConstraintBuilder { // 'submap_id' and the 'range_data_3d.returns' in 'trajectory_nodes' for // 'scan_index'. This performs full-submap matching. // - // The scan at 'scan_index' should be from trajectory 'scan_trajectory', and - // the 'submap' should be from 'submap_trajectory'. The - // 'trajectory_connectivity' is updated if the full-submap match succeeds. + // The scan at 'scan_index' should be from trajectory 'scan_trajectory_id'. + // The 'trajectory_connectivity' is updated if the full-submap match succeeds. // // The pointees of 'submap' and 'range_data_3d.returns' must stay valid until // all computations are finished. void MaybeAddGlobalConstraint( const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, - const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, + int scan_trajectory_id, mapping::TrajectoryConnectivity* trajectory_connectivity, const std::vector& trajectory_nodes); @@ -133,13 +131,12 @@ class ConstraintBuilder { // Runs in a background thread and does computations for an additional // constraint, assuming 'submap' and 'point_cloud' do not change anymore. // If 'match_full_submap' is true, and global localization succeeds, will - // connect 'scan_trajectory' and 'submap_trajectory' in + // connect 'scan_trajectory_id' and 'submap_id.trajectory_id' in // 'trajectory_connectivity'. // As output, it may create a new Constraint in 'constraint'. void ComputeConstraint( const mapping::SubmapId& submap_id, const Submap* submap, int scan_index, - const mapping::Submaps* scan_trajectory, - const mapping::Submaps* submap_trajectory, bool match_full_submap, + int scan_trajectory_id, bool match_full_submap, mapping::TrajectoryConnectivity* trajectory_connectivity, const sensor::CompressedPointCloud* const compressed_point_cloud, const transform::Rigid3d& initial_relative_pose,