12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.
master
Wolfgang Hess 2017-09-13 14:08:32 +02:00 committed by GitHub
parent 57d53b6d53
commit 5ade042520
4 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -305,6 +305,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
for (const Constraint& constraint : result) { for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag, CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
common::Time time = common::Time time =
trajectory_nodes_.at(constraint.node_id).constant_data->time; trajectory_nodes_.at(constraint.node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(constraint.submap_id); 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()); constraints_.insert(constraints_.end(), result.begin(), result.end());
} }
RunOptimization(); RunOptimization();
UpdateTrajectoryConnectivity(result);
common::MutexLocker locker(&mutex_); 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; num_scans_since_last_loop_closure_ = 0;
run_loop_closure_ = false; run_loop_closure_ = false;
while (!run_loop_closure_) { while (!run_loop_closure_) {
@ -479,11 +485,6 @@ void SparsePoseGraph::RunOptimization() {
} }
} }
optimized_submap_transforms_ = submap_data; optimized_submap_transforms_ = submap_data;
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
} }
std::vector<std::vector<mapping::TrajectoryNode>> std::vector<std::vector<mapping::TrajectoryNode>>

3
cartographer/mapping_2d/sparse_pose_graph.h
View File

@ -155,7 +155,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Updates the trajectory connectivity structure with the new constraints. // Updates the trajectory connectivity structure with the new constraints.
void UpdateTrajectoryConnectivity( 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 // Computes the local to global frame transform based on the given optimized
// 'submap_transforms'. // 'submap_transforms'.

13
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -325,6 +325,7 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
for (const Constraint& constraint : result) { for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag, CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP); mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
CHECK(trajectory_nodes_.at(constraint.node_id).constant_data != nullptr);
common::Time time = common::Time time =
trajectory_nodes_.at(constraint.node_id).constant_data->time; trajectory_nodes_.at(constraint.node_id).constant_data->time;
const SubmapData& submap_data = submap_data_.at(constraint.submap_id); 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()); constraints_.insert(constraints_.end(), result.begin(), result.end());
} }
RunOptimization(); RunOptimization();
UpdateTrajectoryConnectivity(result);
common::MutexLocker locker(&mutex_); 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; num_scans_since_last_loop_closure_ = 0;
run_loop_closure_ = false; run_loop_closure_ = false;
while (!run_loop_closure_) { while (!run_loop_closure_) {
@ -526,11 +532,6 @@ void SparsePoseGraph::RunOptimization() {
} }
optimized_submap_transforms_ = submap_data; optimized_submap_transforms_ = submap_data;
TrimmingHandle trimming_handle(this);
for (auto& trimmer : trimmers_) {
trimmer->Trim(&trimming_handle);
}
// Log the histograms for the pose residuals. // Log the histograms for the pose residuals.
if (options_.log_residual_histograms()) { if (options_.log_residual_histograms()) {
LogResidualHistograms(); LogResidualHistograms();

3
cartographer/mapping_3d/sparse_pose_graph.h
View File

@ -170,7 +170,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
// Updates the trajectory connectivity structure with the new constraints. // Updates the trajectory connectivity structure with the new constraints.
void UpdateTrajectoryConnectivity( void UpdateTrajectoryConnectivity(
const sparse_pose_graph::ConstraintBuilder::Result& result); const sparse_pose_graph::ConstraintBuilder::Result& result)
REQUIRES(mutex_);
const mapping::proto::SparsePoseGraphOptions options_; const mapping::proto::SparsePoseGraphOptions options_;
common::Mutex mutex_; common::Mutex mutex_;