Remove 2D scan matcher covariances. (#307)

This replaces the scaled covariances derived from the Ceres
scan matcher by directly configurable weights. Using covariances
did not provide the expected benefit, and replacing the scaling
matrix by two values will allow a faster evaluation of the cost
function in the future.

cartographer/mapping/proto/sparse_pose_graph_options.proto

@@ -28,6 +28,11 @@ 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
+  // constraints.
+  optional double matcher_translation_weight = 7;
+  optional double matcher_rotation_weight = 8;
+
   // Options for the optimization problem.
   optional mapping.sparse_pose_graph.proto.OptimizationProblemOptions
       optimization_problem_options = 4;

cartographer/mapping/sparse_pose_graph.cc

@@ -16,7 +16,6 @@
 
 #include "cartographer/mapping/sparse_pose_graph.h"
 
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping/sparse_pose_graph/constraint_builder.h"
 #include "cartographer/mapping/sparse_pose_graph/optimization_problem_options.h"
 #include "cartographer/transform/transform.h"
@@ -44,6 +43,10 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
   *options.mutable_constraint_builder_options() =
       sparse_pose_graph::CreateConstraintBuilderOptions(
           parameter_dictionary->GetDictionary("constraint_builder").get());
+  options.set_matcher_translation_weight(
+      parameter_dictionary->GetDouble("matcher_translation_weight"));
+  options.set_matcher_rotation_weight(
+      parameter_dictionary->GetDouble("matcher_rotation_weight"));
   *options.mutable_optimization_problem_options() =
       sparse_pose_graph::CreateOptimizationProblemOptions(
           parameter_dictionary->GetDictionary("optimization_problem").get());
@@ -55,6 +58,20 @@ proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
   return options;
 }
 
+Eigen::Matrix<double, 6, 6> FromTranslationRotationWeights(
+    const double translation_weight, const double rotation_weight) {
+  Eigen::Matrix<double, 6, 6> result;
+  // clang-format off
+  result << translation_weight, 0., 0., 0., 0., 0.,
+            0., translation_weight, 0., 0., 0., 0.,
+            0., 0., translation_weight, 0., 0., 0.,
+            0., 0., 0.,    rotation_weight, 0., 0.,
+            0., 0., 0., 0.,    rotation_weight, 0.,
+            0., 0., 0., 0., 0.,    rotation_weight;
+  // clang-format on
+  return result;
+}
+
 proto::SparsePoseGraph SparsePoseGraph::ToProto() {
   proto::SparsePoseGraph proto;
 

cartographer/mapping/sparse_pose_graph.h

@@ -35,6 +35,10 @@ namespace mapping {
 proto::SparsePoseGraphOptions CreateSparsePoseGraphOptions(
     common::LuaParameterDictionary* const parameter_dictionary);
 
+// TODO(whess): Change to two doubles for performance.
+Eigen::Matrix<double, 6, 6> FromTranslationRotationWeights(
+    double translation_weight, double rotation_weight);
+
 class SparsePoseGraph {
  public:
   // A "constraint" as in the paper by Konolige, Kurt, et al. "Efficient sparse

cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -40,6 +40,10 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
       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(
+      parameter_dictionary->GetDouble("loop_closure_rotation_weight"));
   options.set_log_matches(parameter_dictionary->GetBool("log_matches"));
   *options.mutable_fast_correlative_scan_matcher_options() =
       mapping_2d::scan_matching::CreateFastCorrelativeScanMatcherOptions(

cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -42,8 +42,13 @@ message ConstraintBuilderOptions {
   optional double global_localization_min_score = 5;
 
   // Lower bound for covariance eigenvalues to limit the weight of matches.
+  // TODO(whess): Deprecated and only used in 3D. Replace usage and remove.
   optional double lower_covariance_eigenvalue_bound = 7;
 
+  // Weights used in the optimization problem for loop closure constraints.
+  optional double loop_closure_translation_weight = 13;
+  optional double loop_closure_rotation_weight = 14;
+
   // If enabled, logs information of loop-closing constraints for debugging.
   optional bool log_matches = 8;
 

cartographer/mapping_2d/global_trajectory_builder.cc

@@ -42,10 +42,8 @@ void GlobalTrajectoryBuilder::AddRangefinderData(
     sparse_pose_graph_->AddScan(
         insertion_result->time, insertion_result->tracking_to_tracking_2d,
         insertion_result->range_data_in_tracking_2d,
-        insertion_result->pose_estimate_2d,
+        insertion_result->pose_estimate_2d, trajectory_id_,
-        kalman_filter::Project2D(insertion_result->covariance_estimate),
+        insertion_result->matching_submap, insertion_result->insertion_submaps);
-        trajectory_id_, insertion_result->matching_submap,
-        insertion_result->insertion_submaps);
   }
 }
 

cartographer/mapping_2d/local_trajectory_builder.cc

@@ -69,8 +69,7 @@ void LocalTrajectoryBuilder::ScanMatch(
     common::Time time, const transform::Rigid3d& pose_prediction,
     const transform::Rigid3d& tracking_to_tracking_2d,
     const sensor::RangeData& range_data_in_tracking_2d,
-    transform::Rigid3d* pose_observation,
+    transform::Rigid3d* pose_observation) {
-    kalman_filter::PoseCovariance* covariance_observation) {
   const ProbabilityGrid& probability_grid =
       submaps_.Get(submaps_.matching_index())->probability_grid;
   transform::Rigid2d pose_prediction_2d =
@@ -89,21 +88,13 @@ void LocalTrajectoryBuilder::ScanMatch(
   }
 
   transform::Rigid2d tracking_2d_to_map;
-  kalman_filter::Pose2DCovariance covariance_observation_2d;
   ceres::Solver::Summary summary;
   ceres_scan_matcher_.Match(pose_prediction_2d, initial_ceres_pose,
                             filtered_point_cloud_in_tracking_2d,
-                            probability_grid, &tracking_2d_to_map,
+                            probability_grid, &tracking_2d_to_map, &summary);
-                            &covariance_observation_2d, &summary);
 
   *pose_observation =
       transform::Embed3D(tracking_2d_to_map) * tracking_to_tracking_2d;
-  // This covariance is used for non-yaw rotation and the fake height of 0.
-  constexpr double kFakePositionCovariance = 1.;
-  constexpr double kFakeOrientationCovariance = 1.;
-  *covariance_observation =
-      kalman_filter::Embed3D(covariance_observation_2d, kFakePositionCovariance,
-                             kFakeOrientationCovariance);
 }
 
 std::unique_ptr<LocalTrajectoryBuilder::InsertionResult>
@@ -144,10 +135,8 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
     return nullptr;
   }
 
-  kalman_filter::PoseCovariance covariance_observation;
   ScanMatch(time, pose_prediction, tracking_to_tracking_2d,
-            range_data_in_tracking_2d, &pose_estimate_,
+            range_data_in_tracking_2d, &pose_estimate_);
-            &covariance_observation);
   odometry_correction_ = transform::Rigid3d::Identity();
   if (!odometry_state_tracker_.empty()) {
     // We add an odometry state, so that the correction from the scan matching
@@ -199,7 +188,7 @@ LocalTrajectoryBuilder::AddHorizontalRangeData(
 
   return common::make_unique<InsertionResult>(InsertionResult{
       time, matching_submap, insertion_submaps, tracking_to_tracking_2d,
-      range_data_in_tracking_2d, pose_estimate_2d, covariance_observation});
+      range_data_in_tracking_2d, pose_estimate_2d});
 }
 
 const mapping::GlobalTrajectoryBuilderInterface::PoseEstimate&

cartographer/mapping_2d/local_trajectory_builder.h

@@ -46,7 +46,6 @@ class LocalTrajectoryBuilder {
     transform::Rigid3d tracking_to_tracking_2d;
     sensor::RangeData range_data_in_tracking_2d;
     transform::Rigid2d pose_estimate_2d;
-    kalman_filter::PoseCovariance covariance_estimate;
   };
 
   explicit LocalTrajectoryBuilder(
@@ -71,13 +70,12 @@ class LocalTrajectoryBuilder {
       const transform::Rigid3f& tracking_to_tracking_2d,
       const sensor::RangeData& range_data) const;
 
-  // Scan match 'range_data_in_tracking_2d' and fill in the
+  // Scan matches 'range_data_in_tracking_2d' and fill in the 'pose_observation'
-  // 'pose_observation' and 'covariance_observation' with the result.
+  // with the result.
   void ScanMatch(common::Time time, const transform::Rigid3d& pose_prediction,
                  const transform::Rigid3d& tracking_to_tracking_2d,
                  const sensor::RangeData& range_data_in_tracking_2d,
-                 transform::Rigid3d* pose_observation,
+                 transform::Rigid3d* pose_observation);
-                 kalman_filter::PoseCovariance* covariance_observation);
 
   // Lazily constructs an ImuTracker.
   void InitializeImuTracker(common::Time time);

cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc

@@ -22,7 +22,6 @@
 #include "Eigen/Core"
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/scan_matching/occupied_space_cost_functor.h"
 #include "cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h"
@@ -44,8 +43,6 @@ proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(
       parameter_dictionary->GetDouble("translation_weight"));
   options.set_rotation_weight(
       parameter_dictionary->GetDouble("rotation_weight"));
-  options.set_covariance_scale(
-      parameter_dictionary->GetDouble("covariance_scale"));
   *options.mutable_ceres_solver_options() =
       common::CreateCeresSolverOptionsProto(
           parameter_dictionary->GetDictionary("ceres_solver_options").get());
@@ -67,7 +64,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
                              const sensor::PointCloud& point_cloud,
                              const ProbabilityGrid& probability_grid,
                              transform::Rigid2d* const pose_estimate,
-                             kalman_filter::Pose2DCovariance* const covariance,
                              ceres::Solver::Summary* const summary) const {
   double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                    initial_pose_estimate.translation().y(),
@@ -100,17 +96,6 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
 
   *pose_estimate = transform::Rigid2d(
       {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
-
-  ceres::Covariance::Options options;
-  ceres::Covariance covariance_computer(options);
-  std::vector<std::pair<const double*, const double*>> covariance_blocks;
-  covariance_blocks.emplace_back(ceres_pose_estimate, ceres_pose_estimate);
-  CHECK(covariance_computer.Compute(covariance_blocks, &problem));
-  double ceres_covariance[3 * 3];
-  covariance_computer.GetCovarianceBlock(ceres_pose_estimate,
-                                         ceres_pose_estimate, ceres_covariance);
-  *covariance = Eigen::Map<kalman_filter::Pose2DCovariance>(ceres_covariance);
-  *covariance *= options_.covariance_scale();
 }
 
 }  // namespace scan_matching

cartographer/mapping_2d/scan_matching/ceres_scan_matcher.h

@@ -22,7 +22,6 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/lua_parameter_dictionary.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
 #include "cartographer/sensor/point_cloud.h"
@@ -45,14 +44,13 @@ class CeresScanMatcher {
   CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
 
   // Aligns 'point_cloud' within the 'probability_grid' given an
-  // 'initial_pose_estimate' and returns 'pose_estimate', 'covariance', and
+  // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
-  // the solver 'summary'.
+  // 'summary'.
   void Match(const transform::Rigid2d& previous_pose,
              const transform::Rigid2d& initial_pose_estimate,
              const sensor::PointCloud& point_cloud,
              const ProbabilityGrid& probability_grid,
              transform::Rigid2d* pose_estimate,
-             kalman_filter::Pose2DCovariance* covariance,
              ceres::Solver::Summary* summary) const;
 
  private:

cartographer/mapping_2d/scan_matching/ceres_scan_matcher_test.cc

@@ -47,7 +47,6 @@ class CeresScanMatcherTest : public ::testing::Test {
           occupied_space_weight = 1.,
           translation_weight = 0.1,
           rotation_weight = 1.5,
-          covariance_scale = 10.,
           ceres_solver_options = {
             use_nonmonotonic_steps = true,
             max_num_iterations = 50,
@@ -61,12 +60,11 @@ class CeresScanMatcherTest : public ::testing::Test {
 
   void TestFromInitialPose(const transform::Rigid2d& initial_pose) {
     transform::Rigid2d pose;
-    kalman_filter::Pose2DCovariance covariance;
     const transform::Rigid2d expected_pose =
         transform::Rigid2d::Translation({-0.5, 0.5});
     ceres::Solver::Summary summary;
     ceres_scan_matcher_->Match(initial_pose, initial_pose, point_cloud_,
-                               probability_grid_, &pose, &covariance, &summary);
+                               probability_grid_, &pose, &summary);
     EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
     EXPECT_THAT(pose, transform::IsNearly(expected_pose, 1e-2))
         << "Actual: " << transform::ToProto(pose).DebugString()

cartographer/mapping_2d/scan_matching/real_time_correlative_scan_matcher_test.cc

@@ -22,7 +22,6 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
 #include "cartographer/common/make_unique.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/range_data_inserter.h"
 #include "cartographer/sensor/point_cloud.h"

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -90,8 +90,7 @@ void SparsePoseGraph::GrowSubmapTransformsAsNeeded(
 void SparsePoseGraph::AddScan(
     common::Time time, const transform::Rigid3d& tracking_to_pose,
     const sensor::RangeData& range_data_in_pose, const transform::Rigid2d& pose,
-    const kalman_filter::Pose2DCovariance& covariance, const int trajectory_id,
+    const int trajectory_id, const mapping::Submap* const matching_submap,
-    const mapping::Submap* const matching_submap,
     const std::vector<const mapping::Submap*>& insertion_submaps) {
   const transform::Rigid3d optimized_pose(
       GetLocalToGlobalTransform(trajectory_id) * transform::Embed3D(pose));
@@ -128,7 +127,7 @@ void SparsePoseGraph::AddScan(
 
   AddWorkItem([=]() REQUIRES(mutex_) {
     ComputeConstraintsForScan(matching_submap, insertion_submaps,
-                              finished_submap, pose, covariance);
+                              finished_submap, pose);
   });
 }
 
@@ -207,8 +206,7 @@ void SparsePoseGraph::ComputeConstraintsForOldScans(
 void SparsePoseGraph::ComputeConstraintsForScan(
     const mapping::Submap* matching_submap,
     std::vector<const mapping::Submap*> insertion_submaps,
-    const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
+    const mapping::Submap* finished_submap, const transform::Rigid2d& pose) {
-    const kalman_filter::Pose2DCovariance& covariance) {
   GrowSubmapTransformsAsNeeded(insertion_submaps);
   const mapping::SubmapId matching_id = GetSubmapId(matching_submap);
   const transform::Rigid2d optimized_pose =
@@ -234,20 +232,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::Rigid2d constraint_transform =
         sparse_pose_graph::ComputeSubmapPose(*submap).inverse() * pose;
-    constexpr double kFakePositionCovariance = 1e-6;
+    constraints_.push_back(
-    constexpr double kFakeOrientationCovariance = 1e-6;
+        Constraint{submap_id,
-    constraints_.push_back(Constraint{
-        submap_id,
                    node_id,
                    {transform::Embed3D(constraint_transform),
-         common::ComputeSpdMatrixSqrtInverse(
+                    mapping::FromTranslationRotationWeights(
-             kalman_filter::Embed3D(covariance, kFakePositionCovariance,
+                        options_.matcher_translation_weight(),
-                                    kFakeOrientationCovariance),
+                        options_.matcher_rotation_weight())},
-             options_.constraint_builder_options()
-                 .lower_covariance_eigenvalue_bound())},
                    Constraint::INTRA_SUBMAP});
   }
 

cartographer/mapping_2d/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_2d/sparse_pose_graph/constraint_builder.h"
@@ -68,9 +67,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   // into the 'insertion_submaps'.
   void AddScan(common::Time time, const transform::Rigid3d& tracking_to_pose,
                const sensor::RangeData& range_data_in_pose,
-               const transform::Rigid2d& pose,
+               const transform::Rigid2d& pose, int trajectory_id,
-               const kalman_filter::Pose2DCovariance& pose_covariance,
+               const mapping::Submap* matching_submap,
-               int trajectory_id, const mapping::Submap* matching_submap,
                const std::vector<const mapping::Submap*>& insertion_submaps)
       EXCLUDES(mutex_);
 
@@ -125,8 +123,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
   void ComputeConstraintsForScan(
       const mapping::Submap* matching_submap,
       std::vector<const mapping::Submap*> insertion_submaps,
-      const mapping::Submap* finished_submap, const transform::Rigid2d& pose,
+      const mapping::Submap* finished_submap, const transform::Rigid2d& pose)
-      const kalman_filter::Pose2DCovariance& covariance) REQUIRES(mutex_);
+      REQUIRES(mutex_);
 
   // Computes constraints for a scan and submap pair.
   void ComputeConstraint(const mapping::NodeId& node_id,

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -29,7 +29,6 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/thread_pool.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
 #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
 #include "cartographer/transform/transform.h"
@@ -212,24 +211,19 @@ void ConstraintBuilder::ComputeConstraint(
   // effect that, in the absence of better information, we prefer the original
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
-  kalman_filter::Pose2DCovariance covariance;
   ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
                             *submap_scan_matcher->probability_grid,
-                            &pose_estimate, &covariance, &unused_summary);
+                            &pose_estimate, &unused_summary);
-  // 'covariance' is unchanged as (submap <- map) is a translation.
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;
-  constexpr double kFakePositionCovariance = 1e-6;
+  constraint->reset(
-  constexpr double kFakeOrientationCovariance = 1e-6;
+      new Constraint{submap_id,
-  constraint->reset(new Constraint{
-      submap_id,
                      node_id,
                      {transform::Embed3D(constraint_transform),
-       common::ComputeSpdMatrixSqrtInverse(
+                      mapping::FromTranslationRotationWeights(
-           kalman_filter::Embed3D(covariance, kFakePositionCovariance,
+                          options_.loop_closure_translation_weight(),
-                                  kFakeOrientationCovariance),
+                          options_.loop_closure_rotation_weight())},
-           options_.lower_covariance_eigenvalue_bound())},
                      Constraint::INTER_SUBMAP});
 
   if (options_.log_matches()) {
@@ -245,9 +239,7 @@ void ConstraintBuilder::ComputeConstraint(
            << difference.translation().norm() << " rotation "
            << std::setprecision(3) << std::abs(difference.normalized_angle());
     }
-    info << " with score " << std::setprecision(1) << 100. * score
+    info << " with score " << std::setprecision(1) << 100. * score << "%.";
-         << "% covariance trace " << std::scientific << std::setprecision(4)
-         << covariance.trace() << ".";
     LOG(INFO) << info.str();
   }
 }

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -79,6 +79,8 @@ 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,
               fast_correlative_scan_matcher = {
                 linear_search_window = 3.,
@@ -89,7 +91,6 @@ class SparsePoseGraphTest : public ::testing::Test {
                 occupied_space_weight = 20.,
                 translation_weight = 10.,
                 rotation_weight = 1.,
-                covariance_scale = 1.,
                 ceres_solver_options = {
                   use_nonmonotonic_steps = true,
                   max_num_iterations = 50,
@@ -117,6 +118,8 @@ class SparsePoseGraphTest : public ::testing::Test {
                 },
               },
             },
+            matcher_translation_weight = 1.,
+            matcher_rotation_weight = 1.,
             optimization_problem = {
               acceleration_weight = 1.,
               rotation_weight = 1e2,
@@ -147,8 +150,6 @@ class SparsePoseGraphTest : public ::testing::Test {
     const sensor::PointCloud new_point_cloud = sensor::TransformPointCloud(
         point_cloud_,
         transform::Embed3D(current_pose_.inverse().cast<float>()));
-    kalman_filter::Pose2DCovariance covariance =
-        kalman_filter::Pose2DCovariance::Identity();
     const mapping::Submap* const matching_submap =
         submaps_->Get(submaps_->matching_index());
     std::vector<const mapping::Submap*> insertion_submaps;
@@ -161,10 +162,9 @@ class SparsePoseGraphTest : public ::testing::Test {
     constexpr int kTrajectoryId = 0;
     submaps_->InsertRangeData(TransformRangeData(
         range_data, transform::Embed3D(pose_estimate.cast<float>())));
-    sparse_pose_graph_->AddScan(common::FromUniversal(0),
+    sparse_pose_graph_->AddScan(
-                                transform::Rigid3d::Identity(), range_data,
+        common::FromUniversal(0), transform::Rigid3d::Identity(), range_data,
-                                pose_estimate, covariance, kTrajectoryId,
+        pose_estimate, kTrajectoryId, matching_submap, insertion_submaps);
-                                matching_submap, insertion_submaps);
   }
 
   void MoveRelative(const transform::Rigid2d& movement) {

cartographer/mapping_3d/scan_matching/ceres_scan_matcher.h

@@ -46,8 +46,8 @@ class CeresScanMatcher {
   CeresScanMatcher& operator=(const CeresScanMatcher&) = delete;
 
   // Aligns 'point_clouds' within the 'hybrid_grids' given an
-  // 'initial_pose_estimate' and returns 'pose_estimate', and
+  // 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
-  // the solver 'summary'.
+  // 'summary'.
   void Match(const transform::Rigid3d& previous_pose,
              const transform::Rigid3d& initial_pose_estimate,
              const std::vector<PointCloudAndHybridGridPointers>&

configuration_files/sparse_pose_graph.lua

@@ -25,6 +25,8 @@ 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,
     fast_correlative_scan_matcher = {
       linear_search_window = 7.,
@@ -35,7 +37,6 @@ SPARSE_POSE_GRAPH = {
       occupied_space_weight = 20.,
       translation_weight = 10.,
       rotation_weight = 1.,
-      covariance_scale = 1e-4,
       ceres_solver_options = {
         use_nonmonotonic_steps = true,
         max_num_iterations = 10,
@@ -63,6 +64,8 @@ SPARSE_POSE_GRAPH = {
       },
     },
   },
+  matcher_translation_weight = 5e2,
+  matcher_rotation_weight = 1.6e3,
   optimization_problem = {
     huber_scale = 1e1,
     acceleration_weight = 1e4,

configuration_files/trajectory_builder_2d.lua

@@ -39,7 +39,6 @@ TRAJECTORY_BUILDER_2D = {
     occupied_space_weight = 1e1,
     translation_weight = 1e1,
     rotation_weight = 1e2,
-    covariance_scale = 1e-2,
     ceres_solver_options = {
       use_nonmonotonic_steps = false,
       max_num_iterations = 20,