diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 5dbda25..f37ea30 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -79,7 +79,7 @@ proto::SparsePoseGraph SparsePoseGraph::ToProto() { for (size_t old_node_index = 0; old_node_index != single_trajectory_nodes.size(); ++old_node_index) { const auto& node = single_trajectory_nodes[old_node_index]; - if (!node.trimmed()) { + if (node.constant_data != nullptr) { node_id_remapping[NodeId{static_cast(trajectory_id), static_cast(old_node_index)}] = NodeId{static_cast(trajectory_id), diff --git a/cartographer/mapping/trajectory_node.h b/cartographer/mapping/trajectory_node.h index 49d3937..dea4d32 100644 --- a/cartographer/mapping/trajectory_node.h +++ b/cartographer/mapping/trajectory_node.h @@ -51,7 +51,6 @@ struct TrajectoryNode { }; common::Time time() const { return constant_data->time; } - bool trimmed() const { return constant_data == nullptr; } // This must be a shared_ptr. If the data is used for visualization while the // node is being trimmed, it must survive until all use finishes. diff --git a/cartographer/mapping_2d/sparse_pose_graph.cc b/cartographer/mapping_2d/sparse_pose_graph.cc index ace52f9..9ab257e 100644 --- a/cartographer/mapping_2d/sparse_pose_graph.cc +++ b/cartographer/mapping_2d/sparse_pose_graph.cc @@ -202,7 +202,6 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { const mapping::NodeId& node_id = node_id_data.id; - CHECK(!trajectory_nodes_.at(node_id).trimmed()); if (submap_data.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); } @@ -649,10 +648,10 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id); - // Mark the 'nodes_to_remove' as trimmed and remove their data. + // Remove the 'nodes_to_remove' from the sparse pose graph and the + // optimization problem. for (const mapping::NodeId& node_id : nodes_to_remove) { - CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed()); - parent_->trajectory_nodes_.at(node_id).constant_data.reset(); + parent_->trajectory_nodes_.Trim(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id); } } diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 946ed96..5a1880f 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -228,7 +228,6 @@ void SparsePoseGraph::ComputeConstraintsForOldScans( const auto& submap_data = submap_data_.at(submap_id); for (const auto& node_id_data : optimization_problem_.node_data()) { const mapping::NodeId& node_id = node_id_data.id; - CHECK(!trajectory_nodes_.at(node_id).trimmed()); if (submap_data.node_ids.count(node_id) == 0) { ComputeConstraint(node_id, submap_id); } @@ -676,10 +675,10 @@ void SparsePoseGraph::TrimmingHandle::MarkSubmapAsTrimmed( parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_.TrimSubmap(submap_id); - // Mark the 'nodes_to_remove' as trimmed and remove their data. + // Remove the 'nodes_to_remove' from the sparse pose graph and the + // optimization problem. for (const mapping::NodeId& node_id : nodes_to_remove) { - CHECK(!parent_->trajectory_nodes_.at(node_id).trimmed()); - parent_->trajectory_nodes_.at(node_id).constant_data.reset(); + parent_->trajectory_nodes_.Trim(node_id); parent_->optimization_problem_.TrimTrajectoryNode(node_id); } }