From 96b71e227f53f3fdd796d6e5796ffd94eb741e2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Sch=C3=BCtte?= Date: Fri, 25 Aug 2017 11:39:56 +0200 Subject: [PATCH] Add histograms for pose residuals (#478) * WIP Add histograms for pose residuals * Address reviewers comments * Address more comments. --- .../proto/sparse_pose_graph_options.proto | 3 ++ cartographer/mapping/sparse_pose_graph.cc | 2 ++ .../mapping_2d/sparse_pose_graph_test.cc | 1 + cartographer/mapping_3d/sparse_pose_graph.cc | 33 +++++++++++++++++++ cartographer/mapping_3d/sparse_pose_graph.h | 4 +++ configuration_files/sparse_pose_graph.lua | 1 + docs/source/configuration.rst | 3 ++ 7 files changed, 47 insertions(+) diff --git a/cartographer/mapping/proto/sparse_pose_graph_options.proto b/cartographer/mapping/proto/sparse_pose_graph_options.proto index 67c77a7..410590c 100644 --- a/cartographer/mapping/proto/sparse_pose_graph_options.proto +++ b/cartographer/mapping/proto/sparse_pose_graph_options.proto @@ -47,4 +47,7 @@ message SparsePoseGraphOptions { // Rate at which we sample a single trajectory's scans for global // localization. optional double global_sampling_ratio = 5; + + // Whether to output histograms for the pose residuals. + optional bool log_residual_histograms = 9; } diff --git a/cartographer/mapping/sparse_pose_graph.cc b/cartographer/mapping/sparse_pose_graph.cc index 17361cd..16bc8c8 100644 --- a/cartographer/mapping/sparse_pose_graph.cc +++ b/cartographer/mapping/sparse_pose_graph.cc @@ -55,6 +55,8 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions( CHECK_GT(options.max_num_final_iterations(), 0); options.set_global_sampling_ratio( parameter_dictionary->GetDouble("global_sampling_ratio")); + options.set_log_residual_histograms( + parameter_dictionary->GetBool("log_residual_histograms")); return options; } diff --git a/cartographer/mapping_2d/sparse_pose_graph_test.cc b/cartographer/mapping_2d/sparse_pose_graph_test.cc index 5f58885..da18246 100644 --- a/cartographer/mapping_2d/sparse_pose_graph_test.cc +++ b/cartographer/mapping_2d/sparse_pose_graph_test.cc @@ -145,6 +145,7 @@ class SparsePoseGraphTest : public ::testing::Test { }, max_num_final_iterations = 200, global_sampling_ratio = 0.01, + log_residual_histograms = true, })text"); sparse_pose_graph_ = common::make_unique( mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()), diff --git a/cartographer/mapping_3d/sparse_pose_graph.cc b/cartographer/mapping_3d/sparse_pose_graph.cc index 821eea6..9f9ef10 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.cc +++ b/cartographer/mapping_3d/sparse_pose_graph.cc @@ -433,6 +433,34 @@ void SparsePoseGraph::RunFinalOptimization() { .max_num_iterations()); } +void SparsePoseGraph::LogResidualHistograms() { + common::Histogram rotational_residual; + common::Histogram translational_residual; + for (const Constraint& constraint : constraints_) { + if (constraint.tag == Constraint::Tag::INTRA_SUBMAP) { + const cartographer::transform::Rigid3d optimized_node_to_map = + trajectory_nodes_.at(constraint.node_id).pose; + const cartographer::transform::Rigid3d node_to_submap_constraint = + constraint.pose.zbar_ij; + const cartographer::transform::Rigid3d optimized_submap_to_map = + optimized_submap_transforms_.at(constraint.submap_id.trajectory_id) + .at(constraint.submap_id.submap_index) + .pose; + const cartographer::transform::Rigid3d optimized_node_to_submap = + optimized_submap_to_map.inverse() * optimized_node_to_map; + const cartographer::transform::Rigid3d residual = + node_to_submap_constraint.inverse() * optimized_node_to_submap; + rotational_residual.Add( + common::NormalizeAngleDifference(transform::GetAngle(residual))); + translational_residual.Add(residual.translation().norm()); + } + } + LOG(INFO) << "Translational residuals histogram:\n" + << translational_residual.ToString(10); + LOG(INFO) << "Rotational residuals histogram:\n" + << rotational_residual.ToString(10); +} + void SparsePoseGraph::RunOptimization() { if (optimization_problem_.submap_data().empty()) { return; @@ -481,6 +509,11 @@ void SparsePoseGraph::RunOptimization() { for (auto& trimmer : trimmers_) { trimmer->Trim(&trimming_handle); } + + // Log the histograms for the pose residuals. + if (options_.log_residual_histograms()) { + LogResidualHistograms(); + } } std::vector> diff --git a/cartographer/mapping_3d/sparse_pose_graph.h b/cartographer/mapping_3d/sparse_pose_graph.h index 1f64a78..efdb268 100644 --- a/cartographer/mapping_3d/sparse_pose_graph.h +++ b/cartographer/mapping_3d/sparse_pose_graph.h @@ -161,6 +161,10 @@ class SparsePoseGraph : public mapping::SparsePoseGraph { mapping::SparsePoseGraph::SubmapData GetSubmapDataUnderLock( const mapping::SubmapId& submap_id) REQUIRES(mutex_); + // Logs histograms for the translational and rotational residual of scan + // poses. + void LogResidualHistograms(); + const mapping::proto::SparsePoseGraphOptions options_; common::Mutex mutex_; diff --git a/configuration_files/sparse_pose_graph.lua b/configuration_files/sparse_pose_graph.lua index e7d5ce7..e8e2a30 100644 --- a/configuration_files/sparse_pose_graph.lua +++ b/configuration_files/sparse_pose_graph.lua @@ -94,4 +94,5 @@ SPARSE_POSE_GRAPH = { }, max_num_final_iterations = 200, global_sampling_ratio = 0.003, + log_residual_histograms = true, } diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index c0b1d92..dbfb469 100644 --- a/docs/source/configuration.rst +++ b/docs/source/configuration.rst @@ -78,6 +78,9 @@ double global_sampling_ratio Rate at which we sample a single trajectory's scans for global localization. +bool log_residual_histograms + Whether to output histograms for the pose residuals. + cartographer.mapping.proto.TrajectoryBuilderOptions ===================================================