Remove covariance computation from branch-and-bound. (#27)

These covariances were only used in 2D and are only an estimate.
Following 3D, we change 2D to use the (local) covariance computed
using Ceres.

cartographer/mapping_2d/scan_matching/CMakeLists.txt

@@ -58,7 +58,6 @@ google_library(mapping_2d_scan_matching_fast_correlative_scan_matcher
   DEPENDS
     common_math
     common_port
-    kalman_filter_pose_tracker
     mapping_2d_probability_grid
     mapping_2d_scan_matching_correlative_scan_matcher
     mapping_2d_scan_matching_proto_fast_correlative_scan_matcher_options
@@ -74,7 +73,6 @@ google_library(mapping_2d_scan_matching_fast_global_localizer
   HDRS
     fast_global_localizer.h
   DEPENDS
-    kalman_filter_pose_tracker
     mapping_2d_scan_matching_fast_correlative_scan_matcher
     sensor_voxel_filter
 )

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.cc

@@ -24,7 +24,6 @@
 
 #include "Eigen/Geometry"
 #include "cartographer/common/math.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/transform.h"
@@ -86,8 +85,6 @@ CreateFastCorrelativeScanMatcherOptions(
       parameter_dictionary->GetDouble("angular_search_window"));
   options.set_branch_and_bound_depth(
       parameter_dictionary->GetInt("branch_and_bound_depth"));
-  options.set_covariance_scale(
-      parameter_dictionary->GetDouble("covariance_scale"));
   return options;
 }
 
@@ -212,20 +209,18 @@ FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
 bool FastCorrelativeScanMatcher::Match(
     const transform::Rigid2d& initial_pose_estimate,
     const sensor::PointCloud2D& point_cloud, const float min_score,
-    float* score, transform::Rigid2d* pose_estimate,
-    kalman_filter::Pose2DCovariance* covariance) const {
+    float* score, transform::Rigid2d* pose_estimate) const {
   const SearchParameters search_parameters(options_.linear_search_window(),
                                            options_.angular_search_window(),
                                            point_cloud, limits_.resolution());
   return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
-                                   point_cloud, min_score, score, pose_estimate,
-                                   covariance);
+                                   point_cloud, min_score, score,
+                                   pose_estimate);
 }
 
 bool FastCorrelativeScanMatcher::MatchFullSubmap(
     const sensor::PointCloud2D& point_cloud, float min_score, float* score,
-    transform::Rigid2d* pose_estimate,
-    kalman_filter::Pose2DCovariance* covariance) const {
+    transform::Rigid2d* pose_estimate) const {
   // Compute a search window around the center of the submap that includes it
   // fully.
   const SearchParameters search_parameters(
@@ -238,18 +233,16 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
           Eigen::Vector2d(limits_.cell_limits().num_y_cells,
                           limits_.cell_limits().num_x_cells));
   return MatchWithSearchParameters(search_parameters, center, point_cloud,
-                                   min_score, score, pose_estimate, covariance);
+                                   min_score, score, pose_estimate);
 }
 
 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
     SearchParameters search_parameters,
     const transform::Rigid2d& initial_pose_estimate,
     const sensor::PointCloud2D& point_cloud, float min_score, float* score,
-    transform::Rigid2d* pose_estimate,
-    kalman_filter::Pose2DCovariance* covariance) const {
+    transform::Rigid2d* pose_estimate) const {
   CHECK_NOTNULL(score);
   CHECK_NOTNULL(pose_estimate);
-  CHECK_NOTNULL(covariance);
 
   const Eigen::Rotation2Dd initial_rotation = initial_pose_estimate.rotation();
   const sensor::PointCloud2D rotated_point_cloud =
@@ -266,22 +259,15 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
 
   const std::vector<Candidate> lowest_resolution_candidates =
       ComputeLowestResolutionCandidates(discrete_scans, search_parameters);
-  // The following are intermediate results for computing covariance. See
-  // "Real-Time Correlative Scan Matching" by Olson.
-  Eigen::Matrix3d K = Eigen::Matrix3d::Zero();
-  Eigen::Vector3d u = Eigen::Vector3d::Zero();
-  double s = 0.;
   const Candidate best_candidate = BranchAndBound(
       discrete_scans, search_parameters, lowest_resolution_candidates,
-      precomputation_grid_stack_->max_depth(), min_score, &K, &u, &s);
+      precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
     *score = best_candidate.score;
     *pose_estimate = transform::Rigid2d(
         {initial_pose_estimate.translation().x() + best_candidate.x,
          initial_pose_estimate.translation().y() + best_candidate.y},
         initial_rotation * Eigen::Rotation2Dd(best_candidate.orientation));
-    *covariance = (1. / s) * K - (1. / (s * s)) * (u * u.transpose());
-    *covariance *= options_.covariance_scale();
     return true;
   }
   return false;
@@ -361,16 +347,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
     const std::vector<DiscreteScan>& discrete_scans,
     const SearchParameters& search_parameters,
     const std::vector<Candidate>& candidates, const int candidate_depth,
-    float min_score, Eigen::Matrix3d* K, Eigen::Vector3d* u, double* s) const {
+    float min_score) const {
   if (candidate_depth == 0) {
-    // Update the covariance computation intermediate values.
-    for (const Candidate& candidate : candidates) {
-      const double p = candidate.score;
-      const Eigen::Vector3d xi(candidate.x, candidate.y, candidate.orientation);
-      *K += (xi * xi.transpose()) * p;
-      *u += xi * p;
-      *s += p;
-    }
     // Return the best candidate.
     return *candidates.begin();
   }
@@ -405,7 +383,7 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
         best_high_resolution_candidate,
         BranchAndBound(discrete_scans, search_parameters,
                        higher_resolution_candidates, candidate_depth - 1,
-                       best_high_resolution_candidate.score, K, u, s));
+                       best_high_resolution_candidate.score));
   }
   return best_high_resolution_candidate;
 }

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher.h

@@ -30,7 +30,6 @@
 
 #include "Eigen/Core"
 #include "cartographer/common/port.h"
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "cartographer/mapping_2d/probability_grid.h"
 #include "cartographer/mapping_2d/scan_matching/correlative_scan_matcher.h"
 #include "cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
@@ -109,20 +108,18 @@ class FastCorrelativeScanMatcher {
 
   // Aligns 'point_cloud' within the 'probability_grid' given an
   // 'initial_pose_estimate'. If a score above 'min_score' (excluding equality)
-  // is possible, true is returned, and 'score', 'pose_estimate' and
-  // 'covariance' are updated with the result.
+  // is possible, true is returned, and 'score' and 'pose_estimate' are updated
+  // with the result.
   bool Match(const transform::Rigid2d& initial_pose_estimate,
              const sensor::PointCloud2D& point_cloud, float min_score,
-             float* score, transform::Rigid2d* pose_estimate,
-             kalman_filter::Pose2DCovariance* covariance) const;
+             float* score, transform::Rigid2d* pose_estimate) const;
 
   // Aligns 'point_cloud' within the full 'probability_grid', i.e., not
   // restricted to the configured search window. If a score above 'min_score'
-  // (excluding equality) is possible, true is returned, and 'score',
-  // 'pose_estimate' and 'covariance' are updated with the result.
+  // (excluding equality) is possible, true is returned, and 'score' and
+  // 'pose_estimate' are updated with the result.
   bool MatchFullSubmap(const sensor::PointCloud2D& point_cloud, float min_score,
-                       float* score, transform::Rigid2d* pose_estimate,
-                       kalman_filter::Pose2DCovariance* covariance) const;
+                       float* score, transform::Rigid2d* pose_estimate) const;
 
  private:
   // The actual implementation of the scan matcher, called by Match() and
@@ -132,8 +129,7 @@ class FastCorrelativeScanMatcher {
       SearchParameters search_parameters,
       const transform::Rigid2d& initial_pose_estimate,
       const sensor::PointCloud2D& point_cloud, float min_score, float* score,
-      transform::Rigid2d* pose_estimate,
-      kalman_filter::Pose2DCovariance* covariance) const;
+      transform::Rigid2d* pose_estimate) const;
   std::vector<Candidate> ComputeLowestResolutionCandidates(
       const std::vector<DiscreteScan>& discrete_scans,
       const SearchParameters& search_parameters) const;
@@ -146,9 +142,7 @@ class FastCorrelativeScanMatcher {
   Candidate BranchAndBound(const std::vector<DiscreteScan>& discrete_scans,
                            const SearchParameters& search_parameters,
                            const std::vector<Candidate>& candidates,
-                           int candidate_depth, float min_score,
-                           Eigen::Matrix3d* K, Eigen::Vector3d* u,
-                           double* s) const;
+                           int candidate_depth, float min_score) const;
 
   const proto::FastCorrelativeScanMatcherOptions options_;
   MapLimits limits_;

cartographer/mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -111,7 +111,6 @@ CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
       return {
          linear_search_window = 3.,
          angular_search_window = 1.,
-         covariance_scale = 1.,
          branch_and_bound_depth = )text" +
                              std::to_string(branch_and_bound_depth) + "}");
   return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
@@ -159,11 +158,10 @@ TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
     FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
                                                              options);
     transform::Rigid2d pose_estimate;
-    kalman_filter::Pose2DCovariance unused_covariance;
     float score;
     EXPECT_TRUE(fast_correlative_scan_matcher.Match(
         transform::Rigid2d::Identity(), point_cloud, kMinScore, &score,
-        &pose_estimate, &unused_covariance));
+        &pose_estimate));
     EXPECT_LT(kMinScore, score);
     EXPECT_THAT(expected_pose,
                 transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
@@ -211,10 +209,9 @@ TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
     FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
                                                              options);
     transform::Rigid2d pose_estimate;
-    kalman_filter::Pose2DCovariance unused_covariance;
     float score;
     EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
-        point_cloud, kMinScore, &score, &pose_estimate, &unused_covariance));
+        point_cloud, kMinScore, &score, &pose_estimate));
     EXPECT_LT(kMinScore, score);
     EXPECT_THAT(expected_pose,
                 transform::IsNearly(pose_estimate.cast<float>(), 0.03f))

cartographer/mapping_2d/scan_matching/fast_global_localizer.cc

@@ -16,7 +16,6 @@
 
 #include "cartographer/mapping_2d/scan_matching/fast_global_localizer.h"
 
-#include "cartographer/kalman_filter/pose_tracker.h"
 #include "glog/logging.h"
 
 namespace cartographer {
@@ -36,7 +35,6 @@ bool PerformGlobalLocalization(
   CHECK(best_score != nullptr) << "Need a non-null best_score!";
   *best_score = cutoff;
   transform::Rigid2d pose_estimate;
-  kalman_filter::Pose2DCovariance best_pose_estimate_covariance;
   const sensor::PointCloud2D filtered_point_cloud =
       voxel_filter.Filter(point_cloud);
   bool success = false;
@@ -47,13 +45,11 @@ bool PerformGlobalLocalization(
   for (auto& matcher : matchers) {
     float score = -1;
     transform::Rigid2d pose_estimate;
-    kalman_filter::Pose2DCovariance pose_estimate_covariance;
     if (matcher->MatchFullSubmap(filtered_point_cloud, *best_score, &score,
-                                 &pose_estimate, &pose_estimate_covariance)) {
+                                 &pose_estimate)) {
       CHECK_GT(score, *best_score) << "MatchFullSubmap lied!";
       *best_score = score;
       *best_pose_estimate = pose_estimate;
-      best_pose_estimate_covariance = pose_estimate_covariance;
       success = true;
     }
   }

cartographer/mapping_2d/scan_matching/proto/fast_correlative_scan_matcher_options.proto

@@ -27,7 +27,4 @@ message FastCorrelativeScanMatcherOptions {
 
   // Number of precomputed grids to use.
   optional int32 branch_and_bound_depth = 2;
-
-  // Covariance estimate is multiplied by this scale.
-  optional double covariance_scale = 5;
 };

cartographer/mapping_2d/sparse_pose_graph/constraint_builder.cc

@@ -191,25 +191,23 @@ void ConstraintBuilder::ComputeConstraint(
   // - the ComputeSubmapPose() (map <- i)
   float score = 0.;
   transform::Rigid2d pose_estimate = transform::Rigid2d::Identity();
-  kalman_filter::Pose2DCovariance covariance;
 
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
             filtered_point_cloud, options_.global_localization_min_score(),
-            &score, &pose_estimate, &covariance)) {
+            &score, &pose_estimate)) {
       trajectory_connectivity->Connect(scan_trajectory, submap_trajectory);
     } else {
       return;
     }
   } else {
     if (!submap_scan_matcher->fast_correlative_scan_matcher->Match(
             initial_pose, filtered_point_cloud, options_.min_score(), &score,
-            &pose_estimate, &covariance)) {
+            &pose_estimate)) {
       return;
     }
     // We've reported a successful local match.
     CHECK_GT(score, options_.min_score());
-    // 'covariance' is unchanged as (submap <- map) is a translation.
     {
       common::MutexLocker locker(&mutex_);
       score_histogram_.Add(score);
@@ -218,15 +216,13 @@ void ConstraintBuilder::ComputeConstraint(
 
   // Use the CSM estimate as both the initial and previous pose. This has the
   // effect that, in the absence of better information, we prefer the original
-  // CSM estimate. We also prefer to use the CSM covariance estimate for loop
-  // closure constraints since it is representative of a larger area (as opposed
-  // to the local Ceres estimate of covariance).
+  // CSM estimate.
   ceres::Solver::Summary unused_summary;
-  kalman_filter::Pose2DCovariance unused_covariance;
+  kalman_filter::Pose2DCovariance covariance;
   ceres_scan_matcher_.Match(pose_estimate, pose_estimate, filtered_point_cloud,
                             *submap_scan_matcher->probability_grid,
-                            &pose_estimate, &unused_covariance,
-                            &unused_summary);
+                            &pose_estimate, &covariance, &unused_summary);
+  // 'covariance' is unchanged as (submap <- map) is a translation.
 
   const transform::Rigid2d constraint_transform =
       ComputeSubmapPose(*submap).inverse() * pose_estimate;

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -85,7 +85,6 @@ class SparsePoseGraphTest : public ::testing::Test {
                 linear_search_window = 3.,
                 angular_search_window = 0.1,
                 branch_and_bound_depth = 3,
-                covariance_scale = 1.,
               },
               ceres_scan_matcher = {
                 occupied_space_cost_functor_weight = 20.,

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -237,9 +237,7 @@ void ConstraintBuilder::ComputeConstraint(
 
   // Use the CSM estimate as both the initial and previous pose. This has the
   // effect that, in the absence of better information, we prefer the original
-  // CSM estimate. We also prefer to use the CSM covariance estimate for loop
-  // closure constraints since it is representative of a larger area (as opposed
-  // to the local Ceres estimate of covariance).
+  // CSM estimate.
   ceres::Solver::Summary unused_summary;
   kalman_filter::PoseCovariance covariance;
   ceres_scan_matcher_.Match(

configuration_files/sparse_pose_graph.lua

@@ -31,13 +31,12 @@ SPARSE_POSE_GRAPH = {
       linear_search_window = 7.,
       angular_search_window = math.rad(30.),
       branch_and_bound_depth = 7,
-      covariance_scale = 1e-6,
     },
     ceres_scan_matcher = {
       occupied_space_cost_functor_weight = 20.,
       previous_pose_translation_delta_cost_functor_weight = 10.,
       initial_pose_estimate_rotation_delta_cost_functor_weight = 1.,
-      covariance_scale = 1e-6,
+      covariance_scale = 1e-4,
       ceres_solver_options = {
         use_nonmonotonic_steps = true,
         max_num_iterations = 10,
@@ -67,7 +66,7 @@ SPARSE_POSE_GRAPH = {
     },
   },
   optimization_problem = {
-    huber_scale = 5e2,
+    huber_scale = 1e1,
     acceleration_scale = 7e4,
     rotation_scale = 3e6,
     consecutive_scan_translation_penalty_factor = 1e5,