Removes max_covariance_trace option. (#25)

This option is no longer really useful to predict outliers.
It is removed and other parameters scaled to minimize the change
in behavior. And related cleanup.

cartographer/common/math.h

@@ -82,21 +82,20 @@ T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
   return ceres::atan2(vector.y(), vector.x());
 }
 
-// Computes (A/'scale')^{-1/2} for A being symmetric, positive-semidefinite.
+// Computes 'A'^{-1/2} for A being symmetric, positive-semidefinite.
 // Eigenvalues of 'A' are clamped to be at least 'lower_eigenvalue_bound'.
 template <int N>
 Eigen::Matrix<double, N, N> ComputeSpdMatrixSqrtInverse(
-    const Eigen::Matrix<double, N, N>& A, const double scale,
-    const double lower_eigenvalue_bound) {
+    const Eigen::Matrix<double, N, N>& A, const double lower_eigenvalue_bound) {
   Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
-      covariance_eigen_solver(A / scale);
+      covariance_eigen_solver(A);
   if (covariance_eigen_solver.info() != Eigen::Success) {
     LOG(WARNING) << "SelfAdjointEigenSolver failed; A =\n" << A;
     return Eigen::Matrix<double, N, N>::Identity();
   }
   // Since we compute the inverse, we do not allow smaller values to avoid
   // infinity and NaN.
-  const double relative_lower_bound = lower_eigenvalue_bound / scale;
+  const double relative_lower_bound = lower_eigenvalue_bound;
   return covariance_eigen_solver.eigenvectors() *
          covariance_eigen_solver.eigenvalues()
              .cwiseMax(relative_lower_bound)

cartographer/mapping/CMakeLists.txt

@@ -37,6 +37,7 @@ google_library(mapping_map_builder
     map_builder.h
   DEPENDS
     common_lua_parameter_dictionary
+    common_make_unique
     common_thread_pool
     mapping_2d_global_trajectory_builder
     mapping_2d_local_trajectory_builder

cartographer/mapping/map_builder.cc

@@ -60,8 +60,7 @@ proto::MapBuilderOptions CreateMapBuilderOptions(
 
 MapBuilder::MapBuilder(const proto::MapBuilderOptions& options,
                        std::deque<TrajectoryNode::ConstantData>* constant_data)
-    : options_(options),
-      thread_pool_(options.num_background_threads()) {
+    : options_(options), thread_pool_(options.num_background_threads()) {
   if (options.use_trajectory_builder_2d()) {
     sparse_pose_graph_2d_ = common::make_unique<mapping_2d::SparsePoseGraph>(
         options_.sparse_pose_graph_options(), &thread_pool_, constant_data);

cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -38,8 +38,6 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
   options.set_min_score(parameter_dictionary->GetDouble("min_score"));
   options.set_global_localization_min_score(
       parameter_dictionary->GetDouble("global_localization_min_score"));
-  options.set_max_covariance_trace(
-      parameter_dictionary->GetDouble("max_covariance_trace"));
   options.set_lower_covariance_eigenvalue_bound(
       parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
   options.set_log_matches(parameter_dictionary->GetBool("log_matches"));

cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -41,10 +41,6 @@ message ConstraintBuilderOptions {
   // Threshold below which global localizations are not trusted.
   optional double global_localization_min_score = 5;
 
-  // Threshold for the covariance trace above which a match is not considered.
-  // High traces indicate that the match is not very unique.
-  optional double max_covariance_trace = 6;
-
   // Lower bound for covariance eigenvalues to limit the weight of matches.
   optional double lower_covariance_eigenvalue_bound = 7;
 

cartographer/mapping_2d/CMakeLists.txt

@@ -126,6 +126,7 @@ google_library(mapping_2d_sparse_pose_graph
     mapping_2d_submaps
     mapping_proto_scan_matching_progress
     mapping_sparse_pose_graph
+    mapping_sparse_pose_graph_proto_constraint_builder_options
     mapping_trajectory_connectivity
     sensor_compressed_point_cloud
     sensor_point_cloud

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -32,6 +32,7 @@
 #include "cartographer/common/math.h"
 #include "cartographer/common/time.h"
 #include "cartographer/mapping/proto/scan_matching_progress.pb.h"
+#include "cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.pb.h"
 #include "cartographer/sensor/compressed_point_cloud.h"
 #include "cartographer/sensor/voxel_filter.h"
 #include "glog/logging.h"
@@ -171,12 +172,14 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid2d constraint_transform =
         sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
-    constraints_.push_back(
-        Constraint2D{submap_index,
-                     scan_index,
-                     {constraint_transform,
-                      constraint_builder_.ComputeSqrtLambda(covariance)},
-                     Constraint2D::INTRA_SUBMAP});
+    constraints_.push_back(Constraint2D{
+        submap_index,
+        scan_index,
+        {constraint_transform,
+         common::ComputeSpdMatrixSqrtInverse(
+             covariance, options_.constraint_builder_options()
+                             .lower_covariance_eigenvalue_bound())},
+        Constraint2D::INTRA_SUBMAP});
   }
 
   // Determine if this scan should be globally localized.

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -129,13 +129,6 @@ void ConstraintBuilder::WhenDone(
       [this, current_computation] { FinishComputation(current_computation); });
 }
 
-Eigen::Matrix3d ConstraintBuilder::ComputeSqrtLambda(
-    const Eigen::Matrix3d& covariance) const {
-  return common::ComputeSpdMatrixSqrtInverse(
-      covariance, options_.max_covariance_trace(),
-      options_.lower_covariance_eigenvalue_bound());
-}
-
 void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
     const int submap_index, const ProbabilityGrid* const submap,
     const std::function<void()> work_item) {
@@ -217,10 +210,6 @@ void ConstraintBuilder::ComputeConstraint(
     // We've reported a successful local match.
     CHECK_GT(score, options_.min_score());
     // 'covariance' is unchanged as (submap <- map) is a translation.
-    if (covariance.trace() > options_.max_covariance_trace()) {
-      return;
-    }
-
     {
       common::MutexLocker locker(&mutex_);
       score_histogram_.Add(score);
@@ -244,7 +233,9 @@ void ConstraintBuilder::ComputeConstraint(
   constraint->reset(new OptimizationProblem::Constraint{
       submap_index,
       scan_index,
-      {constraint_transform, ComputeSqrtLambda(covariance)},
+      {constraint_transform,
+       common::ComputeSpdMatrixSqrtInverse(
+           covariance, options_.lower_covariance_eigenvalue_bound())},
       OptimizationProblem::Constraint::INTER_SUBMAP});
 
   if (options_.log_matches()) {

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.h

@@ -106,9 +106,6 @@ class ConstraintBuilder {
   // computations triggered by MaybeAddConstraint() have finished.
   void WhenDone(std::function<void(const Result&)> callback);
 
-  // Computes the inverse square root of 'covariance'.
-  Eigen::Matrix3d ComputeSqrtLambda(const Eigen::Matrix3d& covariance) const;
-
   // Returns the number of consecutive finished scans.
   int GetNumFinishedScans();
 

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -79,7 +79,6 @@ class SparsePoseGraphTest : public ::testing::Test {
               },
               min_score = 0.5,
               global_localization_min_score = 0.6,
-              max_covariance_trace = 10.,
               lower_covariance_eigenvalue_bound = 1e-6,
               log_matches = true,
               fast_correlative_scan_matcher = {

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -189,10 +189,8 @@ void SparsePoseGraph::ComputeConstraintsForScan(
         scan_index,
         {constraint_transform,
          common::ComputeSpdMatrixSqrtInverse(
-             covariance,
-             options_.constraint_builder_options().max_covariance_trace(),
-             options_.constraint_builder_options()
-                 .lower_covariance_eigenvalue_bound())},
+             covariance, options_.constraint_builder_options()
+                             .lower_covariance_eigenvalue_bound())},
         Constraint3D::INTRA_SUBMAP});
   }
 

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -153,13 +153,6 @@ void ConstraintBuilder::WhenDone(
       [this, current_computation] { FinishComputation(current_computation); });
 }
 
-Eigen::Matrix<double, 6, 6> ConstraintBuilder::ComputeSqrtLambda(
-    const kalman_filter::PoseCovariance& covariance) const {
-  return common::ComputeSpdMatrixSqrtInverse(
-      covariance, options_.max_covariance_trace(),
-      options_.lower_covariance_eigenvalue_bound());
-}
-
 void ConstraintBuilder::ScheduleSubmapScanMatcherConstructionAndQueueWorkItem(
     const int submap_index,
     const std::vector<mapping::TrajectoryNode>& submap_nodes,
@@ -248,24 +241,21 @@ void ConstraintBuilder::ComputeConstraint(
   // closure constraints since it is representative of a larger area (as opposed
   // to the local Ceres estimate of covariance).
   ceres::Solver::Summary unused_summary;
-  // TODO(hrapp): Compute covariance instead of relying on Ceres.
   kalman_filter::PoseCovariance covariance;
   ceres_scan_matcher_.Match(
       pose_estimate, pose_estimate,
       {{&filtered_point_cloud, submap_scan_matcher->hybrid_grid}},
       &pose_estimate, &covariance, &unused_summary);
-
   // 'covariance' is unchanged as (submap <- map) is a translation.
-  if (covariance.trace() > options_.max_covariance_trace()) {
-    return;
-  }
 
   const transform::Rigid3d constraint_transform =
       submap->local_pose().inverse() * pose_estimate;
   constraint->reset(new OptimizationProblem::Constraint{
       submap_index,
       scan_index,
-      {constraint_transform, ComputeSqrtLambda(covariance)},
+      {constraint_transform,
+       common::ComputeSpdMatrixSqrtInverse(
+           covariance, options_.lower_covariance_eigenvalue_bound())},
       OptimizationProblem::Constraint::INTER_SUBMAP});
 
   if (options_.log_matches()) {

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h

@@ -102,10 +102,6 @@ class ConstraintBuilder {
   // computations triggered by MaybeAddConstraint() have finished.
   void WhenDone(std::function<void(const Result&)> callback);
 
-  // Computes the inverse square root of 'covariance'.
-  Eigen::Matrix<double, 6, 6> ComputeSqrtLambda(
-      const kalman_filter::PoseCovariance& covariance) const;
-
   // Returns the number of consecutive finished scans.
   int GetNumFinishedScans();
 

configuration_files/sparse_pose_graph.lua

@@ -25,7 +25,6 @@ SPARSE_POSE_GRAPH = {
     },
     min_score = 0.55,
     global_localization_min_score = 0.6,
-    max_covariance_trace = 1e-8,
     lower_covariance_eigenvalue_bound = 1e-11,
     log_matches = false,
     fast_correlative_scan_matcher = {
@@ -68,11 +67,11 @@ SPARSE_POSE_GRAPH = {
     },
   },
   optimization_problem = {
-    huber_scale = 5e-2,
-    acceleration_scale = 7.,
-    rotation_scale = 3e2,
-    consecutive_scan_translation_penalty_factor = 1e1,
-    consecutive_scan_rotation_penalty_factor = 1e1,
+    huber_scale = 5e2,
+    acceleration_scale = 7e4,
+    rotation_scale = 3e6,
+    consecutive_scan_translation_penalty_factor = 1e5,
+    consecutive_scan_rotation_penalty_factor = 1e5,
     log_solver_summary = false,
     log_residual_histograms = false,
     ceres_solver_options = {