From fd5003b69b14f89b7d9a3865597aaf1c79dfab2b Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 13 Nov 2017 14:19:15 +0100 Subject: [PATCH] Omit odometry penalty within frozen trajectory. (#661) In the case of pure localization, this reduces the size of the optimization problem by a large factor. --- .../mapping_2d/sparse_pose_graph/optimization_problem.cc | 4 ++++ .../mapping_3d/sparse_pose_graph/optimization_problem.cc | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc index 0d1f6ac..5073b75 100644 --- a/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc @@ -168,6 +168,10 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (frozen_trajectories.count(trajectory_id) != 0) { + node_it = trajectory_end; + continue; + } auto prev_node_it = node_it; for (++node_it; node_it != trajectory_end; ++node_it) { diff --git a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc index 78e7e35..8b254a8 100644 --- a/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc +++ b/cartographer/mapping_3d/sparse_pose_graph/optimization_problem.cc @@ -309,6 +309,10 @@ void OptimizationProblem::Solve(const std::vector& constraints, for (auto node_it = node_data_.begin(); node_it != node_data_.end();) { const int trajectory_id = node_it->id.trajectory_id; const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id); + if (frozen_trajectories.count(trajectory_id) != 0) { + node_it = trajectory_end; + continue; + } auto prev_node_it = node_it; for (++node_it; node_it != trajectory_end; ++node_it) {