Also check low resolution match for 3D loop closure. (#468)

Related to #369.

cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -36,6 +36,8 @@ proto::ConstraintBuilderOptions CreateConstraintBuilderOptions(
       sensor::CreateAdaptiveVoxelFilterOptions(
           parameter_dictionary->GetDictionary("adaptive_voxel_filter").get());
   options.set_min_score(parameter_dictionary->GetDouble("min_score"));
+  options.set_min_low_resolution_score(
+      parameter_dictionary->GetDouble("min_low_resolution_score"));
   options.set_global_localization_min_score(
       parameter_dictionary->GetDouble("global_localization_min_score"));
   options.set_loop_closure_translation_weight(

cartographer/mapping/sparse_pose_graph/proto/constraint_builder_options.proto

@@ -23,6 +23,7 @@ import "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.p
 import "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto";
 
 message ConstraintBuilderOptions {
+  // Next ID: 18
   // A constraint will be added if the proportion of added constraints to
   // potential constraints drops below this number.
   optional double sampling_ratio = 1;
@@ -39,6 +40,10 @@ message ConstraintBuilderOptions {
   // Low scores indicate that the scan and map do not look similar.
   optional double min_score = 4;
 
+  // Threshold for the score of the low resolution grid below which a match is
+  // not considered. Only used for 3D.
+  optional double min_low_resolution_score = 17;
+
   // Threshold below which global localizations are not trusted.
   optional double global_localization_min_score = 5;
 

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -75,6 +75,7 @@ class SparsePoseGraphTest : public ::testing::Test {
                 max_range = 50.,
               },
               min_score = 0.5,
+              min_low_resolution_score = 0.5,
               global_localization_min_score = 0.6,
               loop_closure_translation_weight = 1.,
               loop_closure_rotation_weight = 1.,

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -106,12 +106,13 @@ bool FastCorrelativeScanMatcher::Match(
     const transform::Rigid3d& initial_pose_estimate,
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
-    float* const score, transform::Rigid3d* const pose_estimate,
+    const MatchingFunction& matching_function, float* const score,
+    transform::Rigid3d* const pose_estimate,
     float* const rotational_score) const {
   const SearchParameters search_parameters{
       common::RoundToInt(options_.linear_xy_search_window() / resolution_),
       common::RoundToInt(options_.linear_z_search_window() / resolution_),
-      options_.angular_search_window()};
+      options_.angular_search_window(), &matching_function};
   return MatchWithSearchParameters(
       search_parameters, initial_pose_estimate, coarse_point_cloud,
       fine_point_cloud, min_score, score, pose_estimate, rotational_score);
@@ -121,7 +122,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
     const Eigen::Quaterniond& gravity_alignment,
     const sensor::PointCloud& coarse_point_cloud,
     const sensor::PointCloud& fine_point_cloud, const float min_score,
-    float* const score, transform::Rigid3d* const pose_estimate,
+    const MatchingFunction& matching_function, float* const score,
+    transform::Rigid3d* const pose_estimate,
     float* const rotational_score) const {
   const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
                                                  gravity_alignment);
@@ -132,8 +134,8 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
   const int linear_window_size =
       (width_in_voxels_ + 1) / 2 +
       common::RoundToInt(max_point_distance / resolution_ + 0.5f);
-  const SearchParameters search_parameters{linear_window_size,
+  const SearchParameters search_parameters{
-                                           linear_window_size, M_PI};
+      linear_window_size, linear_window_size, M_PI, &matching_function};
   return MatchWithSearchParameters(
       search_parameters, initial_pose_estimate, coarse_point_cloud,
       fine_point_cloud, min_score, score, pose_estimate, rotational_score);
@@ -161,14 +163,10 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
       precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
     *score = best_candidate.score;
-    const auto& discrete_scan = discrete_scans[best_candidate.scan_index];
     *pose_estimate =
-        (transform::Rigid3f(
+        GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>();
-             resolution_ * best_candidate.offset.matrix().cast<float>(),
+    *rotational_score =
-             Eigen::Quaternionf::Identity()) *
+        discrete_scans[best_candidate.scan_index].rotational_score;
-         discrete_scan.pose)
-            .cast<double>();
-    *rotational_score = discrete_scan.rotational_score;
     return true;
   }
   return false;
@@ -334,18 +332,38 @@ FastCorrelativeScanMatcher::ComputeLowestResolutionCandidates(
   return lowest_resolution_candidates;
 }
 
+transform::Rigid3f FastCorrelativeScanMatcher::GetPoseFromCandidate(
+    const std::vector<DiscreteScan>& discrete_scans,
+    const Candidate& candidate) const {
+  return transform::Rigid3f::Translation(
+             resolution_ * candidate.offset.matrix().cast<float>()) *
+         discrete_scans[candidate.scan_index].pose;
+}
+
 Candidate FastCorrelativeScanMatcher::BranchAndBound(
     const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const std::vector<DiscreteScan>& discrete_scans,
     const std::vector<Candidate>& candidates, const int candidate_depth,
     float min_score) const {
-  if (candidate_depth == 0) {
-    // Return the best candidate.
-    return *candidates.begin();
-  }
-
   Candidate best_high_resolution_candidate(0, Eigen::Array3i::Zero());
   best_high_resolution_candidate.score = min_score;
+  if (candidate_depth == 0) {
+    for (const Candidate& candidate : candidates) {
+      if (candidate.score <= min_score) {
+        // Return if the candidate is bad because the following candidate will
+        // not have better score.
+        return best_high_resolution_candidate;
+      } else if ((*search_parameters.matching_function)(
+                     GetPoseFromCandidate(discrete_scans, candidate))) {
+        // We found the best candidate that passes the matching function.
+        return candidate;
+      }
+    }
+
+    // All candidates have good scores but none passes the matching function.
+    return best_high_resolution_candidate;
+  }
+
   for (const Candidate& candidate : candidates) {
     if (candidate.score <= min_score) {
       break;

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -68,6 +68,8 @@ struct Candidate {
   bool operator>(const Candidate& other) const { return score > other.score; }
 };
 
+using MatchingFunction = std::function<bool(const transform::Rigid3f&)>;
+
 class FastCorrelativeScanMatcher {
  public:
   FastCorrelativeScanMatcher(
@@ -88,8 +90,8 @@ class FastCorrelativeScanMatcher {
   bool Match(const transform::Rigid3d& initial_pose_estimate,
              const sensor::PointCloud& coarse_point_cloud,
              const sensor::PointCloud& fine_point_cloud, float min_score,
-             float* score, transform::Rigid3d* pose_estimate,
+             const MatchingFunction& matching_function, float* score,
-             float* rotational_score) const;
+             transform::Rigid3d* pose_estimate, float* rotational_score) const;
 
   // Aligns 'coarse_point_cloud' within the 'hybrid_grid' given a rotation which
   // is expected to be approximately gravity aligned. If a score above
@@ -100,7 +102,8 @@ class FastCorrelativeScanMatcher {
   bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
                        const sensor::PointCloud& coarse_point_cloud,
                        const sensor::PointCloud& fine_point_cloud,
-                       float min_score, float* score,
+                       float min_score,
+                       const MatchingFunction& matching_function, float* score,
                        transform::Rigid3d* pose_estimate,
                        float* rotational_score) const;
 
@@ -109,6 +112,7 @@ class FastCorrelativeScanMatcher {
     const int linear_xy_window_size;     // voxels
     const int linear_z_window_size;      // voxels
     const double angular_search_window;  // radians
+    const MatchingFunction* const matching_function;
   };
 
   bool MatchWithSearchParameters(
@@ -138,6 +142,9 @@ class FastCorrelativeScanMatcher {
                            const std::vector<DiscreteScan>& discrete_scans,
                            const std::vector<Candidate>& candidates,
                            int candidate_depth, float min_score) const;
+  transform::Rigid3f GetPoseFromCandidate(
+      const std::vector<DiscreteScan>& discrete_scans,
+      const Candidate& candidate) const;
 
   const proto::FastCorrelativeScanMatcherOptions options_;
   const float resolution_;

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -22,6 +22,7 @@
 #include <string>
 
 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/common/make_unique.h"
 #include "cartographer/mapping_3d/range_data_inserter.h"
 #include "cartographer/transform/rigid_transform_test_helpers.h"
 #include "cartographer/transform/transform.h"
@@ -32,87 +33,141 @@ namespace mapping_3d {
 namespace scan_matching {
 namespace {
 
-proto::FastCorrelativeScanMatcherOptions
+class FastCorrelativeScanMatcherTest : public ::testing::Test {
-CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
+ protected:
-  auto parameter_dictionary = common::MakeDictionary(
+  FastCorrelativeScanMatcherTest()
-      "return {"
+      : range_data_inserter_(CreateRangeDataInserterTestOptions()),
-      "branch_and_bound_depth = " +
+        options_(CreateFastCorrelativeScanMatcherTestOptions(5)){};
-      std::to_string(branch_and_bound_depth) +
-      ", "
-      "full_resolution_depth = " +
-      std::to_string(branch_and_bound_depth) +
-      ", "
-      "rotational_histogram_size = 30, "
-      "min_rotational_score = 0.1, "
-      "linear_xy_search_window = 0.8, "
-      "linear_z_search_window = 0.8, "
-      "angular_search_window = 0.3, "
-      "}");
-  return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
-}
 
-mapping_3d::proto::RangeDataInserterOptions
+  void SetUp() override {
-CreateRangeDataInserterTestOptions() {
+    point_cloud_ = {
-  auto parameter_dictionary = common::MakeDictionary(
+        Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f),
-      "return { "
+        Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f),
-      "hit_probability = 0.7, "
+        Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f),
-      "miss_probability = 0.4, "
+        Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f),
-      "num_free_space_voxels = 5, "
+        Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f),
-      "}");
+        Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)};
-  return CreateRangeDataInserterOptions(parameter_dictionary.get());
+  }
-}
 
-TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
+  transform::Rigid3f GetRandomPose() {
-  std::mt19937 prng(42);
+    const float x = 0.7f * distribution_(prng_);
-  std::uniform_real_distribution<float> distribution(-1.f, 1.f);
+    const float y = 0.7f * distribution_(prng_);
-  RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
+    const float z = 0.7f * distribution_(prng_);
-  constexpr float kMinScore = 0.1f;
+    const float theta = 0.2f * distribution_(prng_);
-  const auto options = CreateFastCorrelativeScanMatcherTestOptions(5);
+    return transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) *
+           transform::Rigid3f::Rotation(
+               Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
+  }
 
-  sensor::PointCloud point_cloud{
+  static proto::FastCorrelativeScanMatcherOptions
-      Eigen::Vector3f(4.f, 0.f, 0.f), Eigen::Vector3f(4.5f, 0.f, 0.f),
+  CreateFastCorrelativeScanMatcherTestOptions(
-      Eigen::Vector3f(5.f, 0.f, 0.f), Eigen::Vector3f(5.5f, 0.f, 0.f),
+      const int branch_and_bound_depth) {
-      Eigen::Vector3f(0.f, 4.f, 0.f), Eigen::Vector3f(0.f, 4.5f, 0.f),
+    auto parameter_dictionary = common::MakeDictionary(
-      Eigen::Vector3f(0.f, 5.f, 0.f), Eigen::Vector3f(0.f, 5.5f, 0.f),
+        "return {"
-      Eigen::Vector3f(0.f, 0.f, 4.f), Eigen::Vector3f(0.f, 0.f, 4.5f),
+        "branch_and_bound_depth = " +
-      Eigen::Vector3f(0.f, 0.f, 5.f), Eigen::Vector3f(0.f, 0.f, 5.5f)};
+        std::to_string(branch_and_bound_depth) +
+        ", "
+        "full_resolution_depth = " +
+        std::to_string(branch_and_bound_depth) +
+        ", "
+        "rotational_histogram_size = 30, "
+        "min_rotational_score = 0.1, "
+        "linear_xy_search_window = 0.8, "
+        "linear_z_search_window = 0.8, "
+        "angular_search_window = 0.3, "
+        "}");
+    return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
+  }
 
-  for (int i = 0; i != 20; ++i) {
+  static mapping_3d::proto::RangeDataInserterOptions
-    const float x = 0.7f * distribution(prng);
+  CreateRangeDataInserterTestOptions() {
-    const float y = 0.7f * distribution(prng);
+    auto parameter_dictionary = common::MakeDictionary(
-    const float z = 0.7f * distribution(prng);
+        "return { "
-    const float theta = 0.2f * distribution(prng);
+        "hit_probability = 0.7, "
-    const auto expected_pose =
+        "miss_probability = 0.4, "
-        transform::Rigid3f::Translation(Eigen::Vector3f(x, y, z)) *
+        "num_free_space_voxels = 5, "
-        transform::Rigid3f::Rotation(
+        "}");
-            Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()));
+    return CreateRangeDataInserterOptions(parameter_dictionary.get());
+  }
 
+  std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
+      const proto::FastCorrelativeScanMatcherOptions& options,
+      const transform::Rigid3f& pose) {
     HybridGrid hybrid_grid(0.05f);
-    range_data_inserter.Insert(
+    range_data_inserter_.Insert(
-        sensor::RangeData{
+        sensor::RangeData{pose.translation(),
-            expected_pose.translation(),
+                          sensor::TransformPointCloud(point_cloud_, pose),
-            sensor::TransformPointCloud(point_cloud, expected_pose),
+                          {}},
-            {}},
         &hybrid_grid);
     hybrid_grid.FinishUpdate();
 
-    FastCorrelativeScanMatcher fast_correlative_scan_matcher(hybrid_grid, {},
+    return common::make_unique<FastCorrelativeScanMatcher>(
-                                                             options);
+        hybrid_grid, std::vector<mapping::TrajectoryNode>(), options);
+  }
+
+  std::mt19937 prng_ = std::mt19937(42);
+  std::uniform_real_distribution<float> distribution_ =
+      std::uniform_real_distribution<float>(-1.f, 1.f);
+  RangeDataInserter range_data_inserter_;
+  static constexpr float kMinScore = 0.1f;
+  const proto::FastCorrelativeScanMatcherOptions options_;
+  sensor::PointCloud point_cloud_;
+};
+
+constexpr float FastCorrelativeScanMatcherTest::kMinScore;
+
+TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
+  for (int i = 0; i != 20; ++i) {
+    const auto expected_pose = GetRandomPose();
+
+    std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
+        GetFastCorrelativeScanMatcher(options_, expected_pose));
+
     float score = 0.f;
     transform::Rigid3d pose_estimate;
     float rotational_score = 0.f;
-    EXPECT_TRUE(fast_correlative_scan_matcher.Match(
+    EXPECT_TRUE(fast_correlative_scan_matcher->Match(
-        transform::Rigid3d::Identity(), point_cloud, point_cloud, kMinScore,
+        transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
-        &score, &pose_estimate, &rotational_score));
+        [](const transform::Rigid3f&) { return true; }, &score, &pose_estimate,
+        &rotational_score));
     EXPECT_LT(kMinScore, score);
     EXPECT_LT(0.09f, rotational_score);
     EXPECT_THAT(expected_pose,
                 transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
         << "Actual: " << transform::ToProto(pose_estimate).DebugString()
         << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
+    EXPECT_FALSE(fast_correlative_scan_matcher->Match(
+        transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
+        [](const transform::Rigid3f&) { return false; }, &score, &pose_estimate,
+        &rotational_score));
   }
 }
 
+TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
+  const auto expected_pose = GetRandomPose();
+
+  std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
+      GetFastCorrelativeScanMatcher(options_, expected_pose));
+
+  float score = 0.f;
+  transform::Rigid3d pose_estimate;
+  float rotational_score = 0.f;
+  EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
+      Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
+      [](const transform::Rigid3f&) { return true; }, &score, &pose_estimate,
+      &rotational_score));
+  EXPECT_LT(kMinScore, score);
+  EXPECT_LT(0.09f, rotational_score);
+  EXPECT_THAT(expected_pose,
+              transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
+      << "Actual: " << transform::ToProto(pose_estimate).DebugString()
+      << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
+  EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
+      Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
+      [](const transform::Rigid3f&) { return false; }, &score, &pose_estimate,
+      &rotational_score));
+}
+
 }  // namespace
 }  // namespace scan_matching
 }  // namespace mapping_3d

cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc

@@ -0,0 +1,51 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
+
+namespace cartographer {
+namespace mapping_3d {
+namespace scan_matching {
+
+namespace {
+
+// TODO(zhengj, whess): Interpolate the Grid to get better score.
+float EvaluateLowResolutionScore(const HybridGrid& low_resolution_grid,
+                                 const sensor::PointCloud& points) {
+  float score = 0.f;
+  for (const auto& point : points) {
+    score += low_resolution_grid.GetProbability(
+        low_resolution_grid.GetCellIndex(point));
+  }
+  return score / points.size();
+}
+
+}  // namespace
+
+std::function<bool(const transform::Rigid3f&)> CreateLowResolutionMatcher(
+    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points,
+    const float min_low_resolution_score) {
+  return [=](const transform::Rigid3f& pose) {
+    return EvaluateLowResolutionScore(
+               *low_resolution_grid,
+               sensor::TransformPointCloud(*points, pose)) >=
+           min_low_resolution_score;
+  };
+}
+
+}  // namespace scan_matching
+}  // namespace mapping_3d
+}  // namespace cartographer

cartographer/mapping_3d/scan_matching/low_resolution_matcher.h

@@ -0,0 +1,38 @@
+/*
+ * Copyright 2016 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_
+#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_
+
+#include <functional>
+
+#include "cartographer/mapping_3d/hybrid_grid.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+
+namespace cartographer {
+namespace mapping_3d {
+namespace scan_matching {
+
+std::function<bool(const transform::Rigid3f&)> CreateLowResolutionMatcher(
+    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points,
+    float min_low_resolution_score);
+
+}  // namespace scan_matching
+}  // namespace mapping_3d
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_LOW_RESOLUTION_MATCHER_H_

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -29,6 +29,7 @@
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
 #include "cartographer/common/thread_pool.h"
+#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
 #include "cartographer/mapping_3d/scan_matching/proto/ceres_scan_matcher_options.pb.h"
 #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
 #include "cartographer/transform/transform.h"
@@ -179,6 +180,9 @@ void ConstraintBuilder::ComputeConstraint(
   const sensor::PointCloud point_cloud = compressed_point_cloud->Decompress();
   const sensor::PointCloud high_resolution_point_cloud =
       high_resolution_adaptive_voxel_filter_.Filter(point_cloud);
+  const sensor::PointCloud low_resolution_point_cloud =
+      low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
+
   // The 'constraint_transform' (submap i <- scan j) is computed from:
   // - a 'high_resolution_point_cloud' in scan j and
   // - the initial guess 'initial_pose' (submap i <- scan j).
@@ -186,6 +190,10 @@ void ConstraintBuilder::ComputeConstraint(
   transform::Rigid3d pose_estimate;
   float rotational_score = 0.f;
 
+  const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
+      submap_scan_matcher->low_resolution_hybrid_grid,
+      &low_resolution_point_cloud, options_.min_low_resolution_score());
+
   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.
   // 2. Prune if the score is too low.
@@ -193,8 +201,8 @@ void ConstraintBuilder::ComputeConstraint(
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
             initial_pose.rotation(), high_resolution_point_cloud, point_cloud,
-            options_.global_localization_min_score(), &score, &pose_estimate,
+            options_.global_localization_min_score(), low_resolution_matcher,
-            &rotational_score)) {
+            &score, &pose_estimate, &rotational_score)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
@@ -206,7 +214,8 @@ void ConstraintBuilder::ComputeConstraint(
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
             initial_pose, high_resolution_point_cloud, point_cloud,
-            options_.min_score(), &score, &pose_estimate, &rotational_score)) {
+            options_.min_score(), low_resolution_matcher, &score,
+            &pose_estimate, &rotational_score)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
     } else {
@@ -222,9 +231,6 @@ 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.
-  const sensor::PointCloud low_resolution_point_cloud =
-      low_resolution_adaptive_voxel_filter_.Filter(point_cloud);
-
   ceres::Solver::Summary unused_summary;
   transform::Rigid3d constraint_transform;
   ceres_scan_matcher_.Match(pose_estimate, pose_estimate,

configuration_files/sparse_pose_graph.lua

@@ -23,6 +23,7 @@ SPARSE_POSE_GRAPH = {
       max_range = 50.,
     },
     min_score = 0.55,
+    min_low_resolution_score = 0.55,
     global_localization_min_score = 0.6,
     loop_closure_translation_weight = 1.1e4,
     loop_closure_rotation_weight = 1e5,

docs/source/configuration.rst

@@ -96,6 +96,7 @@ cartographer.mapping.sparse_pose_graph.proto.ConstraintBuilderOptions
 =====================================================================
 
 double sampling_ratio
+  Next ID: 18
   A constraint will be added if the proportion of added constraints to
   potential constraints drops below this number.
 
@@ -110,6 +111,10 @@ double min_score
   Threshold for the scan match score below which a match is not considered.
   Low scores indicate that the scan and map do not look similar.
 
+double min_low_resolution_score
+  Threshold for the score of the low resolution grid below which a match is
+  not considered. Only used for 3D.
+
 double global_localization_min_score
   Threshold below which global localizations are not trusted.