Expose low resolution matching scores. (#470)

This is needed in preparation of adding a low resolution score histogram.

cartographer/mapping/sparse_pose_graph/constraint_builder.cc

@@ -36,8 +36,6 @@ 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,7 +23,6 @@ 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;
@@ -40,10 +39,6 @@ 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.cc

@@ -71,8 +71,7 @@ std::vector<mapping::SubmapId> SparsePoseGraph::GrowSubmapTransformsAsNeeded(
   }
   CHECK_EQ(2, insertion_submaps.size());
   const mapping::SubmapId last_submap_id{
-      trajectory_id,
+      trajectory_id, submap_data.at(trajectory_id).rbegin()->first};
-      submap_data.at(trajectory_id).rbegin()->first};
   if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {
     // In this case, 'last_submap_id' is the ID of 'insertions_submaps.front()'
     // and 'insertions_submaps.back()' is new.
@@ -193,9 +192,8 @@ void SparsePoseGraph::ComputeConstraint(const mapping::NodeId& node_id,
               .pose.inverse() *
           optimization_problem_.node_data()
               .at(node_id.trajectory_id)
-              .at(node_id.node_index -
+              .at(node_id.node_index - optimization_problem_.num_trimmed_nodes(
-                  optimization_problem_.num_trimmed_nodes(
+                                           node_id.trajectory_id))
-                      node_id.trajectory_id))
               .point_cloud_pose;
       constraint_builder_.MaybeAddConstraint(
           submap_id, submap_data_.at(submap_id).submap.get(), node_id,
@@ -348,19 +346,21 @@ void SparsePoseGraph::WaitForAllComputations() {
       common::FromSeconds(1.))) {
     std::ostringstream progress_info;
     progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
-                  << 100. * (constraint_builder_.GetNumFinishedScans() -
+                  << 100. *
-                             num_finished_scans_at_start) /
+                         (constraint_builder_.GetNumFinishedScans() -
+                          num_finished_scans_at_start) /
                          (num_trajectory_nodes_ - num_finished_scans_at_start)
                   << "%...";
     std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
   }
   std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
-  constraint_builder_.WhenDone([this, &notification](
+  constraint_builder_.WhenDone(
-      const sparse_pose_graph::ConstraintBuilder::Result& result) {
+      [this, &notification](
-    common::MutexLocker locker(&mutex_);
+          const sparse_pose_graph::ConstraintBuilder::Result& result) {
-    constraints_.insert(constraints_.end(), result.begin(), result.end());
+        common::MutexLocker locker(&mutex_);
-    notification = true;
+        constraints_.insert(constraints_.end(), result.begin(), result.end());
-  });
+        notification = true;
+      });
   locker.Await([&notification]() { return notification; });
 }
 

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -154,8 +154,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
       const int submap_index = index_submap_data.first;
       const SubmapData& submap_data = index_submap_data.second;
 
-      C_submaps[trajectory_id].emplace(
+      C_submaps[trajectory_id].emplace(submap_index,
-              submap_index, FromPose(submap_data.pose));
+                                       FromPose(submap_data.pose));
       problem.AddParameterBlock(
           C_submaps[trajectory_id].at(submap_index).data(), 3);
       if (first_submap || frozen) {

cartographer/mapping_2d/sparse_pose_graph_test.cc

@@ -75,7 +75,6 @@ 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.,
@@ -100,6 +99,7 @@ class SparsePoseGraphTest : public ::testing::Test {
                 full_resolution_depth = 3,
                 rotational_histogram_size = 30,
                 min_rotational_score = 0.1,
+                min_low_resolution_score = 0.5,
                 linear_xy_search_window = 4.,
                 linear_z_search_window = 4.,
                 angular_search_window = 0.1,

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -45,6 +45,8 @@ CreateFastCorrelativeScanMatcherOptions(
       parameter_dictionary->GetInt("rotational_histogram_size"));
   options.set_min_rotational_score(
       parameter_dictionary->GetDouble("min_rotational_score"));
+  options.set_min_low_resolution_score(
+      parameter_dictionary->GetDouble("min_low_resolution_score"));
   options.set_linear_xy_search_window(
       parameter_dictionary->GetDouble("linear_xy_search_window"));
   options.set_linear_z_search_window(
@@ -354,7 +356,8 @@ Candidate FastCorrelativeScanMatcher::BranchAndBound(
         // not have better score.
         return best_high_resolution_candidate;
       } else if ((*search_parameters.matching_function)(
-                     GetPoseFromCandidate(discrete_scans, candidate))) {
+                     GetPoseFromCandidate(discrete_scans, candidate)) >=
+                 options_.min_low_resolution_score()) {
         // We found the best candidate that passes the matching function.
         return candidate;
       }

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -68,7 +68,8 @@ struct Candidate {
   bool operator>(const Candidate& other) const { return score > other.score; }
 };
 
-using MatchingFunction = std::function<bool(const transform::Rigid3f&)>;
+// Used to compute scores between 0 and 1 how well the given pose matches.
+using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
 
 class FastCorrelativeScanMatcher {
  public:

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -72,6 +72,7 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
         ", "
         "rotational_histogram_size = 30, "
         "min_rotational_score = 0.1, "
+        "min_low_resolution_score = 0.5, "
         "linear_xy_search_window = 0.8, "
         "linear_z_search_window = 0.8, "
         "angular_search_window = 0.3, "
@@ -109,12 +110,13 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
   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;
+constexpr float kMinScore = 0.1f;
+constexpr float kPassingLowResolutionScore = 1.f;
+constexpr float kFailingLowResolutionScore = 0.f;
 
 TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
   for (int i = 0; i != 20; ++i) {
@@ -128,8 +130,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
     float rotational_score = 0.f;
     EXPECT_TRUE(fast_correlative_scan_matcher->Match(
         transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
-        [](const transform::Rigid3f&) { return true; }, &score, &pose_estimate,
+        [](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
-        &rotational_score));
+        &score, &pose_estimate, &rotational_score));
     EXPECT_LT(kMinScore, score);
     EXPECT_LT(0.09f, rotational_score);
     EXPECT_THAT(expected_pose,
@@ -138,8 +140,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
         << "\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,
+        [](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
-        &rotational_score));
+        &score, &pose_estimate, &rotational_score));
   }
 }
 
@@ -154,8 +156,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
   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,
+      [](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
-      &rotational_score));
+      &score, &pose_estimate, &rotational_score));
   EXPECT_LT(kMinScore, score);
   EXPECT_LT(0.09f, rotational_score);
   EXPECT_THAT(expected_pose,
@@ -164,8 +166,8 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
       << "\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,
+      [](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
-      &rotational_score));
+      &score, &pose_estimate, &rotational_score));
 }
 
 }  // namespace

cartographer/mapping_3d/scan_matching/low_resolution_matcher.cc

@@ -20,29 +20,16 @@ namespace cartographer {
 namespace mapping_3d {
 namespace scan_matching {
 
-namespace {
+std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
-
+    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points) {
-// 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(
+    float score = 0.f;
-               *low_resolution_grid,
+    for (const auto& point : sensor::TransformPointCloud(*points, pose)) {
-               sensor::TransformPointCloud(*points, pose)) >=
+      // TODO(zhengj, whess): Interpolate the Grid to get better score.
-           min_low_resolution_score;
+      score += low_resolution_grid->GetProbability(
+          low_resolution_grid->GetCellIndex(point));
+    }
+    return score / points->size();
   };
 }
 

cartographer/mapping_3d/scan_matching/low_resolution_matcher.h

@@ -27,9 +27,8 @@ namespace cartographer {
 namespace mapping_3d {
 namespace scan_matching {
 
-std::function<bool(const transform::Rigid3f&)> CreateLowResolutionMatcher(
+std::function<float(const transform::Rigid3f&)> CreateLowResolutionMatcher(
-    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points,
+    const HybridGrid* low_resolution_grid, const sensor::PointCloud* points);
-    float min_low_resolution_score);
 
 }  // namespace scan_matching
 }  // namespace mapping_3d

cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.proto

@@ -30,6 +30,10 @@ message FastCorrelativeScanMatcherOptions {
   // Minimum score for the rotational scan matcher.
   optional double min_rotational_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 = 9;
+
   // Linear search window in the plane orthogonal to gravity in which the best
   // possible scan alignment will be found.
   optional double linear_xy_search_window = 5;

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -192,7 +192,7 @@ void ConstraintBuilder::ComputeConstraint(
 
   const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
       submap_scan_matcher->low_resolution_hybrid_grid,
-      &low_resolution_point_cloud, options_.min_low_resolution_score());
+      &low_resolution_point_cloud);
 
   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.

configuration_files/sparse_pose_graph.lua

@@ -23,7 +23,6 @@ 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,
@@ -48,6 +47,7 @@ SPARSE_POSE_GRAPH = {
       full_resolution_depth = 3,
       rotational_histogram_size = 120,
       min_rotational_score = 0.77,
+      min_low_resolution_score = 0.55,
       linear_xy_search_window = 5.,
       linear_z_search_window = 1.,
       angular_search_window = math.rad(15.),

docs/source/configuration.rst

@@ -96,7 +96,6 @@ 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.
 
@@ -111,10 +110,6 @@ 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.
 
@@ -445,6 +440,10 @@ int32 rotational_histogram_size
 double min_rotational_score
   Minimum score for the rotational scan matcher.
 
+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 linear_xy_search_window
   Linear search window in the plane orthogonal to gravity in which the best
   possible scan alignment will be found.