diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index fe85dce..c533a0d 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -1238,14 +1238,38 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { parent_->data_.constraints = std::move(constraints); } // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. { std::vector constraints; + std::set other_submap_ids_losing_constraints; for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); } } parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } } // Mark the submap with 'submap_id' as trimmed and remove its data. diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index d6410f7..e40da19 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -1218,14 +1218,38 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { parent_->data_.constraints = std::move(constraints); } // Remove all 'data_.constraints' related to 'nodes_to_remove'. + // If the removal lets other submaps lose all their inter-submap constraints, + // delete their corresponding constraint submap matchers to save memory. { std::vector constraints; + std::set other_submap_ids_losing_constraints; for (const Constraint& constraint : parent_->data_.constraints) { if (nodes_to_remove.count(constraint.node_id) == 0) { constraints.push_back(constraint); + } else { + // A constraint to another submap will be removed, mark it as affected. + other_submap_ids_losing_constraints.insert(constraint.submap_id); } } parent_->data_.constraints = std::move(constraints); + // Go through the remaining constraints to ensure we only delete scan + // matchers of other submaps that have no inter-submap constraints left. + for (const Constraint& constraint : parent_->data_.constraints) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + continue; + } else if (other_submap_ids_losing_constraints.count( + constraint.submap_id)) { + // This submap still has inter-submap constraints - ignore it. + other_submap_ids_losing_constraints.erase(constraint.submap_id); + } + } + // Delete scan matchers of the submaps that lost all constraints. + // TODO(wohe): An improvement to this implementation would be to add the + // caching logic at the constraint builder which could keep around only + // recently used scan matchers. + for (const SubmapId& submap_id : other_submap_ids_losing_constraints) { + parent_->constraint_builder_.DeleteScanMatcher(submap_id); + } } // Mark the submap with 'submap_id' as trimmed and remove its data.