From 5ade042520ccbb75fd847094dffc28d660f67f80 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 13 Sep 2017 14:08:32 +0200 Subject: [PATCH] Fix crash in localization. (#526) Updating the connectivity needs the data of nodes for which constraints were added, so we postpone trimming to after the connectivity update. Also makes sure the mutex is held as necessary. --- cartographer/mapping_2d/sparse_pose_graph.cc | 13 +++++++------ cartographer/mapping_2d/sparse_pose_graph.h | 3 ++- cartographer/mapping_3d/sparse_pose_graph.cc | 13 +++++++------ cartographer/mapping_3d/sparse_pose_graph.h | 3 ++- 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index f462557..34c8a7c 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -305,6 +305,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( 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); @@ -328,9 +329,14 @@ void SparsePoseGraph::HandleWorkQueue() { constraints_.insert(constraints_.end(), result.begin(), result.end()); } RunOptimization(); - UpdateTrajectoryConnectivity(result); common::MutexLocker locker(&mutex_); + UpdateTrajectoryConnectivity(result); + TrimmingHandle trimming_handle(this); + for (auto& trimmer : trimmers_) { + trimmer->Trim(&trimming_handle); + } + num_scans_since_last_loop_closure_ = 0; run_loop_closure_ = false; while (!run_loop_closure_) { @@ -479,11 +485,6 @@ void SparsePoseGraph::RunOptimization() { } } optimized_submap_transforms_ = submap_data; - - TrimmingHandle trimming_handle(this); - for (auto& trimmer : trimmers_) { - trimmer->Trim(&trimming_handle); - } } std::vector> diff --git a/cartographer/mapping_2d/sparse_pose_graph.h b/cartographer/mapping_2d/sparse_pose_graph.h index 0741fca..d055623 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.h +++ b/cartographer/mapping_2d/sparse_pose_graph.h @@ -155,7 +155,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Updates the trajectory connectivity structure with the new constraints. void UpdateTrajectoryConnectivity( - const sparse_pose_graph::ConstraintBuilder::Result& result); + const sparse_pose_graph::ConstraintBuilder::Result& result) + REQUIRES(mutex_); // Computes the local to global frame transform based on the given optimized // 'submap_transforms'. diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 1a0e5f0..43775e7 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -325,6 +325,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity( 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); @@ -348,9 +349,14 @@ void SparsePoseGraph::HandleWorkQueue() { constraints_.insert(constraints_.end(), result.begin(), result.end()); } RunOptimization(); - UpdateTrajectoryConnectivity(result); common::MutexLocker locker(&mutex_); + UpdateTrajectoryConnectivity(result); + TrimmingHandle trimming_handle(this); + for (auto& trimmer : trimmers_) { + trimmer->Trim(&trimming_handle); + } + num_scans_since_last_loop_closure_ = 0; run_loop_closure_ = false; while (!run_loop_closure_) { @@ -526,11 +532,6 @@ void SparsePoseGraph::RunOptimization() { } optimized_submap_transforms_ = submap_data; - TrimmingHandle trimming_handle(this); - for (auto& trimmer : trimmers_) { - trimmer->Trim(&trimming_handle); - } - // Log the histograms for the pose residuals. if (options_.log_residual_histograms()) { LogResidualHistograms(); diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index bc47807..a41b312 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -170,7 +170,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { // Updates the trajectory connectivity structure with the new constraints. void UpdateTrajectoryConnectivity( - const sparse_pose_graph::ConstraintBuilder::Result& result); + const sparse_pose_graph::ConstraintBuilder::Result& result) + REQUIRES(mutex_); const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_;