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

Fix crash in localization. (#523) 

This PR fixes a crash when cartographer is run in localization mode due
to the map trajectory not having been properly added to the
ConnectedComponents.
master
Christoph Schütte 2017-09-12 17:04:13 +02:00 committed by Wolfgang Hess
parent 4437d89dd5
commit b383f18365
2 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -378,6 +378,7 @@ void SparsePoseGraph::WaitForAllComputations() {
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0); CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
frozen_trajectories_.insert(trajectory_id); frozen_trajectories_.insert(trajectory_id);
@ -396,6 +397,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose); const transform::Rigid2d initial_pose_2d = transform::Project2D(initial_pose);
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
const mapping::SubmapId submap_id = const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData()); submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr; submap_data_.at(submap_id).submap = submap_ptr;

2
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -400,6 +400,7 @@ void SparsePoseGraph::WaitForAllComputations() {
void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) { void SparsePoseGraph::FreezeTrajectory(const int trajectory_id) {
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) { AddWorkItem([this, trajectory_id]() REQUIRES(mutex_) {
CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0); CHECK_EQ(frozen_trajectories_.count(trajectory_id), 0);
frozen_trajectories_.insert(trajectory_id); frozen_trajectories_.insert(trajectory_id);
@ -417,6 +418,7 @@ void SparsePoseGraph::AddSubmapFromProto(const int trajectory_id,
std::make_shared<const Submap>(submap.submap_3d()); std::make_shared<const Submap>(submap.submap_3d());
common::MutexLocker locker(&mutex_); common::MutexLocker locker(&mutex_);
trajectory_connectivity_state_.Add(trajectory_id);
const mapping::SubmapId submap_id = const mapping::SubmapId submap_id =
submap_data_.Append(trajectory_id, SubmapData()); submap_data_.Append(trajectory_id, SubmapData());
submap_data_.at(submap_id).submap = submap_ptr; submap_data_.at(submap_id).submap = submap_ptr;