From ff69278ba8029736a8c3eabab2b5567d6ef8e5e9 Mon Sep 17 00:00:00 2001 From: Doan Manh Toan Date: Sun, 1 Jun 2025 21:32:38 +0700 Subject: [PATCH] Remove unused landmark observations Signed-off-by: Doan Manh Toan --- .../mapping/internal/2d/pose_graph_2d.cc | 22 +++++++++++++++++++ .../mapping/internal/3d/pose_graph_3d.cc | 22 +++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/cartographer/mapping/internal/2d/pose_graph_2d.cc b/cartographer/mapping/internal/2d/pose_graph_2d.cc index 060277eb56..5ba73ae7c8 100644 --- a/cartographer/mapping/internal/2d/pose_graph_2d.cc +++ b/cartographer/mapping/internal/2d/pose_graph_2d.cc @@ -1282,6 +1282,28 @@ void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_->TrimSubmap(submap_id); + // Removes landmark observations whose timestamps are older than the latest + // timestamp among the 'nodes_to_remove'. + common::Time latest_to_be_removed_node_time; + for (const NodeId& node_id : nodes_to_remove) { + latest_to_be_removed_node_time = + std::max(latest_to_be_removed_node_time, + parent_->data_.trajectory_nodes.at(node_id).time()); + } + for (auto& landmark_node : parent_->data_.landmark_nodes) { + auto& observations = landmark_node.second.landmark_observations; + if (observations.empty()) continue; + observations.erase( + std::remove_if( + observations.begin(), observations.end(), + [&latest_to_be_removed_node_time]( + PoseGraphInterface::LandmarkNode::LandmarkObservation& + observation) { + return observation.time <= latest_to_be_removed_node_time; + }), + observations.end()); + } + // We have one submap less, update the gauge metrics. kDeletedSubmapsMetric->Increment(); if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) { diff --git a/cartographer/mapping/internal/3d/pose_graph_3d.cc b/cartographer/mapping/internal/3d/pose_graph_3d.cc index 8a91e592ec..bf9ccb51db 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d.cc @@ -1262,6 +1262,28 @@ void PoseGraph3D::TrimmingHandle::TrimSubmap(const SubmapId& submap_id) { parent_->constraint_builder_.DeleteScanMatcher(submap_id); parent_->optimization_problem_->TrimSubmap(submap_id); + // Removes landmark observations whose timestamps are older than the latest + // timestamp among the 'nodes_to_remove'. + common::Time latest_to_be_removed_node_time; + for (const NodeId& node_id : nodes_to_remove) { + latest_to_be_removed_node_time = + std::max(latest_to_be_removed_node_time, + parent_->data_.trajectory_nodes.at(node_id).time()); + } + for (auto& landmark_node : parent_->data_.landmark_nodes) { + auto& observations = landmark_node.second.landmark_observations; + if (observations.empty()) continue; + observations.erase( + std::remove_if( + observations.begin(), observations.end(), + [&latest_to_be_removed_node_time]( + PoseGraphInterface::LandmarkNode::LandmarkObservation& + observation) { + return observation.time <= latest_to_be_removed_node_time; + }), + observations.end()); + } + // We have one submap less, update the gauge metrics. kDeletedSubmapsMetric->Increment(); if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id)) {