Add histograms for pose residuals (#478)

* WIP Add histograms for pose residuals

* Address reviewers comments

* Address more comments.

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;
 }

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;
 }
 

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<SparsePoseGraph>(
           mapping::CreateSparsePoseGraphOptions(parameter_dictionary.get()),

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<std::vector<mapping::TrajectoryNode>>

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_;
 

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,
 }

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
 ===================================================