From 5896ead32ec22bf76144d839c4452396f28b7333 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Thu, 14 Sep 2017 12:11:54 +0200 Subject: [PATCH] Introduce timeout for global constraints. (#517) This PR introduces a new option that specifies the number of seconds after which global matcher searches are performed if no recent global constraints have been found between the submap's and the node's trajectory. --- .../proto/sparse_pose_graph_options.proto | 5 ++ cartographer/mapping/sparse_pose_graph.cc | 3 + cartographer/mapping_2d/sparse_pose_graph.cc | 73 ++++++++++------- cartographer/mapping_2d/sparse_pose_graph.h | 3 + .../mapping_2d/sparse_pose_graph_test.cc | 1 + cartographer/mapping_3d/sparse_pose_graph.cc | 79 +++++++++++-------- cartographer/mapping_3d/sparse_pose_graph.h | 4 + configuration_files/sparse_pose_graph.lua | 1 + 8 files changed, 106 insertions(+), 63 deletions(-) diff --git a/cartographer/mapping/proto/sparse_pose_graph_options.proto b/cartographer/mapping/proto/sparse_pose_graph_options.proto index 410590c..36a7df9 100644 --- a/cartographer/mapping/proto/sparse_pose_graph_options.proto +++ b/cartographer/mapping/proto/sparse_pose_graph_options.proto @@ -50,4 +50,9 @@ message SparsePoseGraphOptions { // Whether to output histograms for the pose residuals. optional bool log_residual_histograms = 9; + + // If for the duration specified by this option no global contraint has been + // added between two trajectories, loop closure searches will be performed + // globally rather than in a smaller search window. + optional double global_constraint_search_after_n_seconds = 10; } diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 763d210..4224034 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -57,6 +57,9 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( parameter_dictionary->GetDouble("global_sampling_ratio")); options.set_log_residual_histograms( parameter_dictionary->GetBool("log_residual_histograms")); + options.set_global_constraint_search_after_n_seconds( + parameter_dictionary->GetDouble( + "global_constraint_search_after_n_seconds")); return options; } diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 34c8a7c..6a09ede 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -173,29 +173,35 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, const mapping::SubmapId& submap_id) { CHECK(submap_data_.at(submap_id).state == SubmapState::kFinished); - // Only globally match against submaps not in this trajectory. - if (node_id.trajectory_id != submap_id.trajectory_id && - global_localization_samplers_[node_id.trajectory_id]->Pulse()) { + const common::Time scan_time = GetLatestScanTime(node_id, submap_id); + const common::Time last_connection_time = + trajectory_connectivity_state_.LastConnectionTime( + node_id.trajectory_id, submap_id.trajectory_id); + if (node_id.trajectory_id == submap_id.trajectory_id || + scan_time < + last_connection_time + common::FromSeconds( + options_.global_constraint_search_after_n_seconds())) { + // If the scan and the submap belong to the same trajectory or if there has + // been a recent global constraint that ties that scan's trajectory to the + // submap's trajectory, it suffices to do a match constrained to a local + // search window. + const transform::Rigid2d initial_relative_pose = + optimization_problem_.submap_data() + .at(submap_id.trajectory_id) + .at(submap_id.submap_index) + .pose.inverse() * + optimization_problem_.node_data() + .at(node_id.trajectory_id) + .at(node_id.node_index) + .pose; + constraint_builder_.MaybeAddConstraint( + submap_id, submap_data_.at(submap_id).submap.get(), node_id, + trajectory_nodes_.at(node_id).constant_data.get(), + initial_relative_pose); + } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get()); - } else { - if (trajectory_connectivity_state_.TransitivelyConnected( - node_id.trajectory_id, submap_id.trajectory_id)) { - const transform::Rigid2d initial_relative_pose = - optimization_problem_.submap_data() - .at(submap_id.trajectory_id) - .at(submap_id.submap_index) - .pose.inverse() * - optimization_problem_.node_data() - .at(node_id.trajectory_id) - .at(node_id.node_index) - .pose; - constraint_builder_.MaybeAddConstraint( - submap_id, submap_data_.at(submap_id).submap.get(), node_id, - trajectory_nodes_.at(node_id).constant_data.get(), - initial_relative_pose); - } } } @@ -300,21 +306,28 @@ void SparsePoseGraph::ComputeConstraintsForScan( } } +common::Time SparsePoseGraph::GetLatestScanTime( + const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) const { + common::Time time = + trajectory_nodes_.at(node_id).constant_data->time; + const SubmapData& submap_data = submap_data_.at(submap_id); + if (!submap_data.node_ids.empty()) { + const mapping::NodeId last_submap_node_id = + *submap_data_.at(submap_id).node_ids.rbegin(); + time = std::max( + time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); + } + return time; +} + void SparsePoseGraph::UpdateTrajectoryConnectivity( const sparse_pose_graph::ConstraintBuilder::Result& result) { for (const Constraint& constraint : result) { CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); - CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr); - common::Time time = - trajectory_nodes_.at(constraint.node_id).constant_data->time; - const SubmapData& submap_data = submap_data_.at(constraint.submap_id); - if (!submap_data.node_ids.empty()) { - const mapping::NodeId last_submap_node_id = - *submap_data_.at(constraint.submap_id).node_ids.rbegin(); - time = std::max( - time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); - } + const common::Time time = + GetLatestScanTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, time); diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index d055623..1060742 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -99,6 +99,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { std::vector> GetTrajectoryNodes() override EXCLUDES(mutex_); std::vector constraints() override EXCLUDES(mutex_); + common::Time GetLatestScanTime(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) const + REQUIRES(mutex_); private: // The current state of the submap in the background threads. When this diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 4c15f12..07d9074 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -130,6 +130,7 @@ class SparsePoseGraphTest : public ::testing::Test { max_num_final_iterations = 200, global_sampling_ratio = 0.01, log_residual_histograms = true, + global_constraint_search_after_n_seconds = 10.0, })text"); sparse_pose_graph_ = common::make_unique( mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 43775e7..286b992 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -193,33 +193,39 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id, inverse_submap_pose * trajectory_nodes_.at(submap_node_id).pose}); } - // Only globally match against submaps not in this trajectory. - if (node_id.trajectory_id != submap_id.trajectory_id && - global_localization_samplers_[node_id.trajectory_id]->Pulse()) { - // In this situation, 'initial_relative_pose' is: - // - // submap <- global map 2 <- global map 1 <- tracking - // (agreeing on gravity) - // - // Since they possibly came from two disconnected trajectories, the only - // guaranteed connection between the tracking and the submap frames is - // an agreement on the direction of gravity. Therefore, excluding yaw, - // 'initial_relative_pose.rotation()' is a good estimate of the relative - // orientation of the point cloud in the submap frame. Finding the correct - // yaw component will be handled by the matching procedure in the - // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. - constraint_builder_.MaybeAddGlobalConstraint( + const common::Time scan_time = GetLatestScanTime(node_id, submap_id); + const common::Time last_connection_time = + trajectory_connectivity_state_.LastConnectionTime( + node_id.trajectory_id, submap_id.trajectory_id); + if (node_id.trajectory_id == submap_id.trajectory_id || + scan_time < + last_connection_time + common::FromSeconds( + options_.global_constraint_search_after_n_seconds())) { + // If the scan and the submap belong to the same trajectory or if there has + // been a recent global constraint that ties that scan's trajectory to the + // submap's trajectory, it suffices to do a match constrained to a local + // search window. + constraint_builder_.MaybeAddConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - initial_relative_pose.rotation()); - } else { - if (trajectory_connectivity_state_.TransitivelyConnected( - node_id.trajectory_id, submap_id.trajectory_id)) { - constraint_builder_.MaybeAddConstraint( + initial_relative_pose); + } else if (global_localization_samplers_[node_id.trajectory_id]->Pulse()) { + // In this situation, 'initial_relative_pose' is: + // + // submap <- global map 2 <- global map 1 <- tracking + // (agreeing on gravity) + // + // Since they possibly came from two disconnected trajectories, the only + // guaranteed connection between the tracking and the submap frames is + // an agreement on the direction of gravity. Therefore, excluding yaw, + // 'initial_relative_pose.rotation()' is a good estimate of the relative + // orientation of the point cloud in the submap frame. Finding the correct + // yaw component will be handled by the matching procedure in the + // FastCorrelativeScanMatcher, and the given yaw is essentially ignored. + constraint_builder_.MaybeAddGlobalConstraint( submap_id, submap_data_.at(submap_id).submap.get(), node_id, trajectory_nodes_.at(node_id).constant_data.get(), submap_nodes, - initial_relative_pose); - } + initial_relative_pose.rotation()); } } @@ -320,21 +326,28 @@ void SparsePoseGraph::ComputeConstraintsForScan( } } +common::Time SparsePoseGraph::GetLatestScanTime( + const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) const { + common::Time time = + trajectory_nodes_.at(node_id).constant_data->time; + const SubmapData& submap_data = submap_data_.at(submap_id); + if (!submap_data.node_ids.empty()) { + const mapping::NodeId last_submap_node_id = + *submap_data_.at(submap_id).node_ids.rbegin(); + time = std::max( + time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); + } + return time; +} + void SparsePoseGraph::UpdateTrajectoryConnectivity( const sparse_pose_graph::ConstraintBuilder::Result& result) { for (const Constraint& constraint : result) { CHECK_EQ(constraint.tag, mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); - CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr); - common::Time time = - trajectory_nodes_.at(constraint.node_id).constant_data->time; - const SubmapData& submap_data = submap_data_.at(constraint.submap_id); - if (!submap_data.node_ids.empty()) { - const mapping::NodeId last_submap_node_id = - *submap_data_.at(constraint.submap_id).node_ids.rbegin(); - time = std::max( - time, trajectory_nodes_.at(last_submap_node_id).constant_data->time); - } + const common::Time time = + GetLatestScanTime(constraint.node_id, constraint.submap_id); trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, time); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index a41b312..6bdcfad 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -164,6 +164,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); + common::Time GetLatestScanTime(const mapping::NodeId& node_id, + const mapping::SubmapId& submap_id) const + REQUIRES(mutex_); + // Logs histograms for the translational and rotational residual of scan // poses. void LogResidualHistograms() REQUIRES(mutex_); diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index a16d2da..af3109d 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -79,4 +79,5 @@ SPARSE_POSE_GRAPH = { max_num_final_iterations = 200, global_sampling_ratio = 0.003, log_residual_histograms = true, + global_constraint_search_after_n_seconds = 10., }