Also use fixed covariances in 3D SLAM. (#308)

This follows #307.

cartographer/common/math.h

@@ -82,29 +82,6 @@ T atan2(const Eigen::Matrix<T, 2, 1>& vector) {
   return ceres::atan2(vector.y(), vector.x());
 }
 
-// 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 lower_eigenvalue_bound) {
-  Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, N, N>>
-      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;
-  return covariance_eigen_solver.eigenvectors() *
-         covariance_eigen_solver.eigenvalues()
-             .cwiseMax(relative_lower_bound)
-             .cwiseInverse()
-             .cwiseSqrt()
-             .asDiagonal() *
-         covariance_eigen_solver.eigenvectors().inverse();
-}
-
 }  // namespace common
 }  // namespace cartographer
 

cartographer/mapping/proto/sparse_pose_graph_options.proto

@@ -28,9 +28,12 @@ message SparsePoseGraphOptions {
   optional mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
       constraint_builder_options = 3;
 
-  // Weights used in the optimization problem for non-loop-closure scan matcher
+  // Weight used in the optimization problem for the translational component of
-  // constraints.
+  // non-loop-closure scan matcher constraints.
   optional double matcher_translation_weight = 7;
+
+  // Weight used in the optimization problem for the rotational component of
+  // non-loop-closure scan matcher constraints.
   optional double matcher_rotation_weight = 8;
 
   // Options for the optimization problem.

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_lower_covariance_eigenvalue_bound(
-      parameter_dictionary->GetDouble("lower_covariance_eigenvalue_bound"));
   options.set_loop_closure_translation_weight(
       parameter_dictionary->GetDouble("loop_closure_translation_weight"));
   options.set_loop_closure_rotation_weight(

cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -41,12 +41,12 @@ message ConstraintBuilderOptions {
   // Threshold below which global localizations are not trusted.
   optional double global_localization_min_score = 5;
 
-  // Lower bound for covariance eigenvalues to limit the weight of matches.
+  // Weight used in the optimization problem for the translational component of
-  // TODO(whess): Deprecated and only used in 3D. Replace usage and remove.
+  // loop closure constraints.
-  optional double lower_covariance_eigenvalue_bound = 7;
-
-  // Weights used in the optimization problem for loop closure constraints.
   optional double loop_closure_translation_weight = 13;
+
+  // Weight used in the optimization problem for the rotational component of
+  // loop closure constraints.
   optional double loop_closure_rotation_weight = 14;
 
   // If enabled, logs information of loop-closing constraints for debugging.

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -78,7 +78,6 @@ class SparsePoseGraphTest : public ::testing::Test {
               },
               min_score = 0.5,
               global_localization_min_score = 0.6,
-              lower_covariance_eigenvalue_bound = 1e-6,
               loop_closure_translation_weight = 1.,
               loop_closure_rotation_weight = 1.,
               log_matches = true,

cartographer/mapping_3d/global_trajectory_builder.cc

@@ -55,9 +55,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
 
   sparse_pose_graph_->AddScan(
       insertion_result->time, insertion_result->range_data_in_tracking,
-      insertion_result->pose_observation, insertion_result->covariance_estimate,
+      insertion_result->pose_observation, trajectory_id_,
-      trajectory_id_, insertion_result->matching_submap,
+      insertion_result->matching_submap, insertion_result->insertion_submaps);
-      insertion_result->insertion_submaps);
 }
 
 void GlobalTrajectoryBuilder::AddOdometerData(const common::Time time,

cartographer/mapping_3d/kalman_local_trajectory_builder.cc

@@ -178,17 +178,16 @@ KalmanLocalTrajectoryBuilder::AddAccumulatedRangeData(
               .scan_matcher_variance() *
           kalman_filter::PoseCovariance::Identity());
 
-  kalman_filter::PoseCovariance covariance_estimate;
+  kalman_filter::PoseCovariance unused_covariance_estimate;
   pose_tracker_->GetPoseEstimateMeanAndCovariance(
-      time, &scan_matcher_pose_estimate_, &covariance_estimate);
+      time, &scan_matcher_pose_estimate_, &unused_covariance_estimate);
 
   last_pose_estimate_ = {
       time, scan_matcher_pose_estimate_,
       sensor::TransformPointCloud(filtered_range_data.returns,
                                   pose_observation.cast<float>())};
 
-  return InsertIntoSubmap(time, filtered_range_data, pose_observation,
+  return InsertIntoSubmap(time, filtered_range_data, pose_observation);
-                          covariance_estimate);
 }
 
 void KalmanLocalTrajectoryBuilder::AddOdometerData(
@@ -216,8 +215,7 @@ KalmanLocalTrajectoryBuilder::pose_estimate() const {
 std::unique_ptr<KalmanLocalTrajectoryBuilder::InsertionResult>
 KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
     const common::Time time, const sensor::RangeData& range_data_in_tracking,
-    const transform::Rigid3d& pose_observation,
+    const transform::Rigid3d& pose_observation) {
-    const kalman_filter::PoseCovariance& covariance_estimate) {
   if (motion_filter_.IsSimilar(time, pose_observation)) {
     return nullptr;
   }
@@ -231,9 +229,9 @@ KalmanLocalTrajectoryBuilder::InsertIntoSubmap(
       sensor::TransformRangeData(range_data_in_tracking,
                                  pose_observation.cast<float>()),
       pose_tracker_->gravity_orientation());
-  return std::unique_ptr<InsertionResult>(new InsertionResult{
+  return std::unique_ptr<InsertionResult>(
-      time, range_data_in_tracking, pose_observation, covariance_estimate,
+      new InsertionResult{time, range_data_in_tracking, pose_observation,
-      matching_submap, insertion_submaps});
+                          matching_submap, insertion_submaps});
 }
 
 }  // namespace mapping_3d

cartographer/mapping_3d/kalman_local_trajectory_builder.h

@@ -62,8 +62,7 @@ class KalmanLocalTrajectoryBuilder : public LocalTrajectoryBuilderInterface {
 
   std::unique_ptr<InsertionResult> InsertIntoSubmap(
       common::Time time, const sensor::RangeData& range_data_in_tracking,
-      const transform::Rigid3d& pose_observation,
+      const transform::Rigid3d& pose_observation);
-      const kalman_filter::PoseCovariance& covariance_estimate);
 
   const proto::LocalTrajectoryBuilderOptions options_;
   std::unique_ptr<mapping_3d::Submaps> submaps_;

cartographer/mapping_3d/local_trajectory_builder_interface.h

@@ -37,7 +37,6 @@ class LocalTrajectoryBuilderInterface {
     common::Time time;
     sensor::RangeData range_data_in_tracking;
     transform::Rigid3d pose_observation;
-    kalman_filter::PoseCovariance covariance_estimate;
     const Submap* matching_submap;
     std::vector<const Submap*> insertion_submaps;
   };

cartographer/mapping_3d/optimizing_local_trajectory_builder.cc

@@ -19,7 +19,6 @@
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_3d/proto/optimizing_local_trajectory_builder_options.pb.h"
 #include "cartographer/mapping_3d/rotation_cost_function.h"
 #include "cartographer/mapping_3d/scan_matching/occupied_space_cost_functor.h"
@@ -424,12 +423,9 @@ OptimizingLocalTrajectoryBuilder::InsertIntoSubmap(
                                  pose_observation.cast<float>()),
       kFakeGravityOrientation);
 
-  const kalman_filter::PoseCovariance kCovariance =
-      1e-7 * kalman_filter::PoseCovariance::Identity();
-
   return std::unique_ptr<InsertionResult>(
       new InsertionResult{time, range_data_in_tracking, pose_observation,
-                          kCovariance, matching_submap, insertion_submaps});
+                          matching_submap, insertion_submaps});
 }
 
 OptimizingLocalTrajectoryBuilder::State

cartographer/mapping_3d/sparse_pose_graph.cc

@@ -88,8 +88,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 
 void SparsePoseGraph::AddScan(
     common::Time time, const sensor::RangeData& range_data_in_tracking,
-    const transform::Rigid3d& pose,
+    const transform::Rigid3d& pose, const int trajectory_id,
-    const kalman_filter::PoseCovariance& covariance, const int trajectory_id,
     const Submap* const matching_submap,
     const std::vector<const Submap*>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
@@ -124,7 +123,7 @@ void SparsePoseGraph::AddScan(
 
   AddWorkItem([=]() REQUIRES(mutex_) {
     ComputeConstraintsForScan(matching_submap, insertion_submaps,
-                              finished_submap, pose, covariance);
+                              finished_submap, pose);
   });
 }
 
@@ -224,8 +223,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(const Submap* submap) {
 
 void SparsePoseGraph::ComputeConstraintsForScan(
     const Submap* matching_submap, std::vector<const Submap*> insertion_submaps,
-    const Submap* finished_submap, const transform::Rigid3d& pose,
+    const Submap* finished_submap, const transform::Rigid3d& pose) {
-    const kalman_filter::PoseCovariance& covariance) {
   GrowSubmapTransformsAsNeeded(insertion_submaps);
   const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
   const transform::Rigid3d optimized_pose =
@@ -251,17 +249,15 @@ void SparsePoseGraph::ComputeConstraintsForScan(
     const mapping::SubmapId submap_id = GetSubmapId(submap);
     CHECK(!submap_states_.at(submap_id).finished);
     submap_states_.at(submap_id).node_ids.emplace(node_id);
-    // Unchanged covariance as (submap <- map) is a translation.
     const transform::Rigid3d constraint_transform =
         submap->local_pose.inverse() * pose;
-    constraints_.push_back(
+    constraints_.push_back(Constraint{
-        Constraint{submap_id,
+        submap_id,
-                   node_id,
+        node_id,
-                   {constraint_transform,
+        {constraint_transform, mapping::FromTranslationRotationWeights(
-                    common::ComputeSpdMatrixSqrtInverse(
+                                   options_.matcher_translation_weight(),
-                        covariance, options_.constraint_builder_options()
+                                   options_.matcher_rotation_weight())},
-                                        .lower_covariance_eigenvalue_bound())},
+        Constraint::INTRA_SUBMAP});
-                   Constraint::INTRA_SUBMAP});
   }
 
   for (int trajectory_id = 0; trajectory_id < submap_states_.num_trajectories();

cartographer/mapping_3d/sparse_pose_graph.h

@@ -30,7 +30,6 @@
 #include "cartographer/common/mutex.h"
 #include "cartographer/common/thread_pool.h"
 #include "cartographer/common/time.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/sparse_pose_graph.h"
 #include "cartographer/mapping/trajectory_connectivity.h"
 #include "cartographer/mapping_3d/sparse_pose_graph/constraint_builder.h"
@@ -67,9 +66,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // 'insertion_submaps'.
   void AddScan(common::Time time,
                const sensor::RangeData& range_data_in_tracking,
-               const transform::Rigid3d& pose,
+               const transform::Rigid3d& pose, int trajectory_id,
-               const kalman_filter::PoseCovariance& pose_covariance,
+               const Submap* matching_submap,
-               int trajectory_id, const Submap* matching_submap,
                const std::vector<const Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -120,11 +118,11 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
       const std::vector<const Submap*>& insertion_submaps) REQUIRES(mutex_);
 
   // Adds constraints for a scan, and starts scan matching in the background.
-  void ComputeConstraintsForScan(
+  void ComputeConstraintsForScan(const Submap* matching_submap,
-      const Submap* matching_submap,
+                                 std::vector<const Submap*> insertion_submaps,
-      std::vector<const Submap*> insertion_submaps,
+                                 const Submap* finished_submap,
-      const Submap* finished_submap, const transform::Rigid3d& pose,
+                                 const transform::Rigid3d& pose)
-      const kalman_filter::PoseCovariance& covariance) REQUIRES(mutex_);
+      REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
   void ComputeConstraint(const mapping::NodeId& node_id,

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -225,9 +225,9 @@ void ConstraintBuilder::ComputeConstraint(
   constraint->reset(new OptimizationProblem::Constraint{
       submap_id,
       node_id,
-      {constraint_transform,
+      {constraint_transform, mapping::FromTranslationRotationWeights(
-       1. / std::sqrt(options_.lower_covariance_eigenvalue_bound()) *
+                                 options_.loop_closure_translation_weight(),
-           kalman_filter::PoseCovariance::Identity()},
+                                 options_.loop_closure_rotation_weight())},
       OptimizationProblem::Constraint::INTER_SUBMAP});
 
   if (options_.log_matches()) {

configuration_files/sparse_pose_graph.lua

@@ -24,7 +24,6 @@ SPARSE_POSE_GRAPH = {
     },
     min_score = 0.55,
     global_localization_min_score = 0.6,
-    lower_covariance_eigenvalue_bound = 1e-11,
     loop_closure_translation_weight = 1.1e4,
     loop_closure_rotation_weight = 1e5,
     log_matches = true,
@@ -68,8 +67,8 @@ SPARSE_POSE_GRAPH = {
   matcher_rotation_weight = 1.6e3,
   optimization_problem = {
     huber_scale = 1e1,
-    acceleration_weight = 1e4,
+    acceleration_weight = 1e3,
-    rotation_weight = 3e6,
+    rotation_weight = 3e5,
     consecutive_scan_translation_penalty_factor = 1e5,
     consecutive_scan_rotation_penalty_factor = 1e5,
     log_solver_summary = false,

docs/source/configuration.rst

@@ -62,15 +62,9 @@ cartographer.mapping.proto.MapBuilderOptions
 bool use_trajectory_builder_2d
   Not yet documented.
 
-cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options
-  Not yet documented.
-
 bool use_trajectory_builder_3d
   Not yet documented.
 
-cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options
-  Not yet documented.
-
 int32 num_background_threads
   Number of threads to use for background computations.
 
@@ -88,6 +82,14 @@ int32 optimize_every_n_scans
 cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions constraint_builder_options
   Options for the constraint builder.
 
+double matcher_translation_weight
+  Weight used in the optimization problem for the translational component of
+  non-loop-closure scan matcher constraints.
+
+double matcher_rotation_weight
+  Weight used in the optimization problem for the rotational component of
+  non-loop-closure scan matcher constraints.
+
 cartographer.mapping.sparse_pose_graph.proto.OptimizationProblemOptions optimization_problem_options
   Options for the optimization problem.
 
@@ -100,6 +102,16 @@ double global_sampling_ratio
   localization.
 
 
+cartographer.mapping.proto.TrajectoryBuilderOptions
+===================================================
+
+cartographer.mapping_2d.proto.LocalTrajectoryBuilderOptions trajectory_builder_2d_options
+  Not yet documented.
+
+cartographer.mapping_3d.proto.LocalTrajectoryBuilderOptions trajectory_builder_3d_options
+  Not yet documented.
+
+
 cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
 =====================================================================
 
@@ -120,8 +132,13 @@ double min_score
 double global_localization_min_score
   Threshold below which global localizations are not trusted.
 
-double lower_covariance_eigenvalue_bound
+double loop_closure_translation_weight
-  Lower bound for covariance eigenvalues to limit the weight of matches.
+  Weight used in the optimization problem for the translational component of
+  loop closure constraints.
+
+double loop_closure_rotation_weight
+  Weight used in the optimization problem for the rotational component of
+  loop closure constraints.
 
 bool log_matches
   If enabled, logs information of loop-closing constraints for debugging.