From 4437d89dd59264159d8417612bf1164f479ee604 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Tue, 12 Sep 2017 15:36:22 +0200 Subject: [PATCH] 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. --- cartographer/mapping_2d/sparse_pose_graph.cc | 12 ++++++++---- cartographer/mapping_3d/sparse_pose_graph.cc | 14 +++++++++----- 2 files changed, 17 insertions(+), 9 deletions(-) diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index 225fcc7..32af168 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -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); diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index ea2809c..dbecef8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -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);