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

Fix crashing bug during pure localization. (#521) 

When loading a serialized state, no nodes are added for each
submap. This leads to crashes when loop closures are found.
master
Wolfgang Hess 2017-09-12 15:36:22 +02:00 committed by GitHub
parent 23bf0fc0d4
commit 4437d89dd5
2 changed files with 17 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cartographer/mapping_2d/sparse_pose_graph.cc
View File

@ -305,11 +305,15 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
mapping::NodeId last_submap_node_id =
*submap_data_.at(constraint.submap_id).node_ids.rbegin();
common::Time time =
std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time,
trajectory_nodes_.at(last_submap_node_id).constant_data->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);
}
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);

14
cartographer/mapping_3d/sparse_pose_graph.cc
View File

@ -325,11 +325,15 @@ void SparsePoseGraph::UpdateTrajectoryConnectivity(
for (const Constraint& constraint : result) {
CHECK_EQ(constraint.tag,
mapping::SparsePoseGraph::Constraint::INTER_SUBMAP);
const mapping::NodeId last_submap_node_id =
*submap_data_.at(constraint.submap_id).node_ids.rbegin();
const common::Time time =
std::max(trajectory_nodes_.at(constraint.node_id).constant_data->time,
trajectory_nodes_.at(last_submap_node_id).constant_data->time);
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);
}
trajectory_connectivity_state_.Connect(constraint.node_id.trajectory_id,
constraint.submap_id.trajectory_id,
time);