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

Delete submap scan matchers that are unconstrained after trimming. (#1745) 

This improves the memory issue in pure localization mode as
mentioned in issue #1737.

Signed-off-by: Michael Grupp <grupp@magazino.eu>
master
Michael Grupp 2020-10-06 15:41:04 +02:00 committed by GitHub
parent c84da8ec0f
commit 9675d63926
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 48 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
cartographer/mapping/internal/2d/pose_graph_2d.cc
View File

@ -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<Constraint> constraints;
std::set<SubmapId> 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.

24
cartographer/mapping/internal/3d/pose_graph_3d.cc
View File

@ -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<Constraint> constraints;
std::set<SubmapId> 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.