Use TrajectoryNode::Data in FastCorrelativeMatcher. (#493)

* Use TrajectoryNode::Data in FastCorrelativeMatcher.

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -24,6 +24,7 @@
 #include "Eigen/Geometry"
 #include "cartographer/common/make_unique.h"
 #include "cartographer/common/math.h"
+#include "cartographer/mapping_3d/scan_matching/low_resolution_matcher.h"
 #include "cartographer/mapping_3d/scan_matching/precomputation_grid.h"
 #include "cartographer/mapping_3d/scan_matching/proto/fast_correlative_scan_matcher_options.pb.h"
 #include "cartographer/transform/transform.h"
@@ -126,6 +127,7 @@ struct Candidate {
 
 FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
     const HybridGrid& hybrid_grid,
+    const HybridGrid* const low_resolution_hybrid_grid,
     const std::vector<mapping::TrajectoryNode>& nodes,
     const proto::FastCorrelativeScanMatcherOptions& options)
     : options_(options),
@@ -133,49 +135,53 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
       width_in_voxels_(hybrid_grid.grid_size()),
       precomputation_grid_stack_(
           common::make_unique<PrecomputationGridStack>(hybrid_grid, options)),
+      low_resolution_hybrid_grid_(low_resolution_hybrid_grid),
       rotational_scan_matcher_(nodes, options_.rotational_histogram_size()) {}
 
 FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
 
 bool FastCorrelativeScanMatcher::Match(
     const transform::Rigid3d& initial_pose_estimate,
-    const sensor::PointCloud& coarse_point_cloud,
+    const mapping::TrajectoryNode::Data& constant_data, const float min_score,
-    const sensor::PointCloud& fine_point_cloud, const float min_score,
+    float* const score, transform::Rigid3d* const pose_estimate,
-    const MatchingFunction& low_resolution_matcher, float* const score,
+    float* const rotational_score, float* const low_resolution_score) const {
-    transform::Rigid3d* const pose_estimate, float* const rotational_score,
+  const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
-    float* const low_resolution_score) const {
+      low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
   const SearchParameters search_parameters{
       common::RoundToInt(options_.linear_xy_search_window() / resolution_),
       common::RoundToInt(options_.linear_z_search_window() / resolution_),
       options_.angular_search_window(), &low_resolution_matcher};
-  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
+  return MatchWithSearchParameters(
-                                   coarse_point_cloud, fine_point_cloud,
+      search_parameters, initial_pose_estimate,
-                                   min_score, score, pose_estimate,
+      constant_data.high_resolution_point_cloud,
-                                   rotational_score, low_resolution_score);
+      constant_data.range_data.returns.Decompress(), min_score, score,
+      pose_estimate, rotational_score, low_resolution_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchFullSubmap(
     const Eigen::Quaterniond& gravity_alignment,
-    const sensor::PointCloud& coarse_point_cloud,
+    const mapping::TrajectoryNode::Data& constant_data, const float min_score,
-    const sensor::PointCloud& fine_point_cloud, const float min_score,
+    float* const score, transform::Rigid3d* const pose_estimate,
-    const MatchingFunction& low_resolution_matcher, float* const score,
+    float* const rotational_score, float* const low_resolution_score) const {
-    transform::Rigid3d* const pose_estimate, float* const rotational_score,
-    float* const low_resolution_score) const {
   const transform::Rigid3d initial_pose_estimate(Eigen::Vector3d::Zero(),
                                                  gravity_alignment);
   float max_point_distance = 0.f;
-  for (const Eigen::Vector3f& point : coarse_point_cloud) {
+  for (const Eigen::Vector3f& point :
+       constant_data.high_resolution_point_cloud) {
     max_point_distance = std::max(max_point_distance, point.norm());
   }
   const int linear_window_size =
       (width_in_voxels_ + 1) / 2 +
       common::RoundToInt(max_point_distance / resolution_ + 0.5f);
+  const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
+      low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
   const SearchParameters search_parameters{
       linear_window_size, linear_window_size, M_PI, &low_resolution_matcher};
-  return MatchWithSearchParameters(search_parameters, initial_pose_estimate,
+  return MatchWithSearchParameters(
-                                   coarse_point_cloud, fine_point_cloud,
+      search_parameters, initial_pose_estimate,
-                                   min_score, score, pose_estimate,
+      constant_data.high_resolution_point_cloud,
-                                   rotational_score, low_resolution_score);
+      constant_data.range_data.returns.Decompress(), min_score, score,
+      pose_estimate, rotational_score, low_resolution_score);
 }
 
 bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
@@ -269,8 +275,9 @@ std::vector<DiscreteScan> FastCorrelativeScanMatcher::GenerateDiscreteScans(
   }
   const float kSafetyMargin = 1.f - 1e-2f;
   const float angular_step_size =
-      kSafetyMargin * std::acos(1.f - common::Pow2(resolution_) /
+      kSafetyMargin * std::acos(1.f -
-                                          (2.f * common::Pow2(max_scan_range)));
+                                common::Pow2(resolution_) /
+                                    (2.f * common::Pow2(max_scan_range)));
   const int angular_window_size = common::RoundToInt(
       search_parameters.angular_search_window / angular_step_size);
   // TODO(whess): Should there be a small search window for rotations around

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -51,6 +51,7 @@ class FastCorrelativeScanMatcher {
  public:
   FastCorrelativeScanMatcher(
       const HybridGrid& hybrid_grid,
+      const HybridGrid* low_resolution_hybrid_grid,
       const std::vector<mapping::TrajectoryNode>& nodes,
       const proto::FastCorrelativeScanMatcherOptions& options);
   ~FastCorrelativeScanMatcher();
@@ -65,11 +66,9 @@ class FastCorrelativeScanMatcher {
   // 'rotational_score', and 'low_resolution_score' are updated with the result.
   // 'fine_point_cloud' is used to compute the rotational scan matcher score.
   bool Match(const transform::Rigid3d& initial_pose_estimate,
-             const sensor::PointCloud& coarse_point_cloud,
+             const mapping::TrajectoryNode::Data& constant_data,
-             const sensor::PointCloud& fine_point_cloud, float min_score,
+             float min_score, float* score, transform::Rigid3d* pose_estimate,
-             const MatchingFunction& low_resolution_matcher, float* score,
+             float* rotational_score, float* low_resolution_score) const;
-             transform::Rigid3d* pose_estimate, float* rotational_score,
-             float* low_resolution_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
@@ -78,11 +77,9 @@ class FastCorrelativeScanMatcher {
   // are updated with the result. 'fine_point_cloud' is used to compute the
   // rotational scan matcher score.
   bool MatchFullSubmap(const Eigen::Quaterniond& gravity_alignment,
-                       const sensor::PointCloud& coarse_point_cloud,
+                       const mapping::TrajectoryNode::Data& constant_data,
-                       const sensor::PointCloud& fine_point_cloud,
+                       float min_score, float* score,
-                       float min_score,
+                       transform::Rigid3d* pose_estimate,
-                       const MatchingFunction& low_resolution_matcher,
-                       float* score, transform::Rigid3d* pose_estimate,
                        float* rotational_score,
                        float* low_resolution_score) const;
 
@@ -130,6 +127,7 @@ class FastCorrelativeScanMatcher {
   const float resolution_;
   const int width_in_voxels_;
   std::unique_ptr<PrecomputationGridStack> precomputation_grid_stack_;
+  const HybridGrid* const low_resolution_hybrid_grid_;
   RotationalScanMatcher rotational_scan_matcher_;
 };
 

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -72,7 +72,9 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
         ", "
         "rotational_histogram_size = 30, "
         "min_rotational_score = 0.1, "
-        "min_low_resolution_score = 0.5, "
+        // Unknown space has kMinProbability = 0.1, so we need to make sure here
+        // to pick a larger number otherwise we always find matches.
+        "min_low_resolution_score = 0.15, "
         "linear_xy_search_window = 0.8, "
         "linear_z_search_window = 0.8, "
         "angular_search_window = 0.3, "
@@ -94,16 +96,28 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
   std::unique_ptr<FastCorrelativeScanMatcher> GetFastCorrelativeScanMatcher(
       const proto::FastCorrelativeScanMatcherOptions& options,
       const transform::Rigid3f& pose) {
-    HybridGrid hybrid_grid(0.05f);
+    hybrid_grid_ = common::make_unique<HybridGrid>(0.05f);
     range_data_inserter_.Insert(
         sensor::RangeData{pose.translation(),
                           sensor::TransformPointCloud(point_cloud_, pose),
                           {}},
-        &hybrid_grid);
+        hybrid_grid_.get());
-    hybrid_grid.FinishUpdate();
+    hybrid_grid_->FinishUpdate();
 
     return common::make_unique<FastCorrelativeScanMatcher>(
-        hybrid_grid, std::vector<mapping::TrajectoryNode>(), options);
+        *hybrid_grid_, hybrid_grid_.get(),
+        std::vector<mapping::TrajectoryNode>(), options);
+  }
+
+  mapping::TrajectoryNode::Data CreateConstantData(
+      const sensor::PointCloud& low_resolution_point_cloud) {
+    return mapping::TrajectoryNode::Data{
+        common::FromUniversal(0),
+        Compress(sensor::RangeData{Eigen::Vector3f::Zero(), point_cloud_, {}}),
+        {},
+        point_cloud_,
+        low_resolution_point_cloud,
+        transform::Rigid3d::Identity()};
   }
 
   std::mt19937 prng_ = std::mt19937(42);
@@ -112,11 +126,10 @@ class FastCorrelativeScanMatcherTest : public ::testing::Test {
   RangeDataInserter range_data_inserter_;
   const proto::FastCorrelativeScanMatcherOptions options_;
   sensor::PointCloud point_cloud_;
+  std::unique_ptr<HybridGrid> hybrid_grid_;
 };
 
 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) {
@@ -130,20 +143,21 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
     float rotational_score = 0.f;
     float low_resolution_score = 0.f;
     EXPECT_TRUE(fast_correlative_scan_matcher->Match(
-        transform::Rigid3d::Identity(), point_cloud_, point_cloud_, kMinScore,
+        transform::Rigid3d::Identity(), CreateConstantData(point_cloud_),
-        [](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
+        kMinScore, &score, &pose_estimate, &rotational_score,
-        &score, &pose_estimate, &rotational_score, &low_resolution_score));
+        &low_resolution_score));
     EXPECT_LT(kMinScore, score);
     EXPECT_LT(0.09f, rotational_score);
-    EXPECT_LT(0.99f, low_resolution_score);
+    EXPECT_LT(0.14f, low_resolution_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,
+        transform::Rigid3d::Identity(),
-        [](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
+        CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
-        &score, &pose_estimate, &rotational_score, &low_resolution_score));
+        &score, &pose_estimate, &rotational_score, &low_resolution_score))
+        << low_resolution_score;
   }
 }
 
@@ -158,20 +172,21 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
   float rotational_score = 0.f;
   float low_resolution_score = 0.f;
   EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
-      Eigen::Quaterniond::Identity(), point_cloud_, point_cloud_, kMinScore,
+      Eigen::Quaterniond::Identity(), CreateConstantData(point_cloud_),
-      [](const transform::Rigid3f&) { return kPassingLowResolutionScore; },
+      kMinScore, &score, &pose_estimate, &rotational_score,
-      &score, &pose_estimate, &rotational_score, &low_resolution_score));
+      &low_resolution_score));
   EXPECT_LT(kMinScore, score);
   EXPECT_LT(0.09f, rotational_score);
-  EXPECT_LT(0.99f, low_resolution_score);
+  EXPECT_LT(0.14f, low_resolution_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,
+      Eigen::Quaterniond::Identity(),
-      [](const transform::Rigid3f&) { return kFailingLowResolutionScore; },
+      CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
-      &score, &pose_estimate, &rotational_score, &low_resolution_score));
+      &score, &pose_estimate, &rotational_score, &low_resolution_score))
+      << low_resolution_score;
 }
 
 }  // namespace

cartographer/mapping_3d/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/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"
@@ -142,7 +141,8 @@ void ConstraintBuilder::ConstructSubmapScanMatcher(
     const Submap* const submap) {
   auto submap_scan_matcher =
       common::make_unique<scan_matching::FastCorrelativeScanMatcher>(
-          submap->high_resolution_hybrid_grid(), submap_nodes,
+          submap->high_resolution_hybrid_grid(),
+          &submap->low_resolution_hybrid_grid(), submap_nodes,
           options_.fast_correlative_scan_matcher_options_3d());
   common::MutexLocker locker(&mutex_);
   submap_scan_matchers_[submap_id] = {&submap->high_resolution_hybrid_grid(),
@@ -173,8 +173,6 @@ void ConstraintBuilder::ComputeConstraint(
     std::unique_ptr<OptimizationProblem::Constraint>* constraint) {
   const SubmapScanMatcher* const submap_scan_matcher =
       GetSubmapScanMatcher(submap_id);
-  const sensor::PointCloud point_cloud =
-      constant_data->range_data.returns.Decompress();
 
   // The 'constraint_transform' (submap i <- scan j) is computed from:
   // - a 'high_resolution_point_cloud' in scan j and
@@ -184,20 +182,15 @@ void ConstraintBuilder::ComputeConstraint(
   float rotational_score = 0.f;
   float low_resolution_score = 0.f;
 
-  const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
-      submap_scan_matcher->low_resolution_hybrid_grid,
-      &constant_data->low_resolution_point_cloud);
-
   // Compute 'pose_estimate' in three stages:
   // 1. Fast estimate using the fast correlative scan matcher.
   // 2. Prune if the score is too low.
   // 3. Refine.
   if (match_full_submap) {
     if (submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
-            initial_pose.rotation(), constant_data->high_resolution_point_cloud,
+            initial_pose.rotation(), *constant_data,
-            point_cloud, options_.global_localization_min_score(),
+            options_.global_localization_min_score(), &score, &pose_estimate,
-            low_resolution_matcher, &score, &pose_estimate, &rotational_score,
+            &rotational_score, &low_resolution_score)) {
-            &low_resolution_score)) {
       CHECK_GT(score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
@@ -208,8 +201,7 @@ void ConstraintBuilder::ComputeConstraint(
     }
   } else {
     if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
-            initial_pose, constant_data->high_resolution_point_cloud,
+            initial_pose, *constant_data, options_.min_score(), &score,
-            point_cloud, options_.min_score(), low_resolution_matcher, &score,
             &pose_estimate, &rotational_score, &low_resolution_score)) {
       // We've reported a successful local match.
       CHECK_GT(score, options_.min_score());
@@ -274,7 +266,7 @@ void ConstraintBuilder::FinishComputation(const int computation_index) {
       CHECK_EQ(submap_queued_work_items_.size(), 0);
       if (when_done_ != nullptr) {
         for (const std::unique_ptr<OptimizationProblem::Constraint>&
-                 constraint : constraints_) {
+             constraint : constraints_) {
           if (constraint != nullptr) {
             result.push_back(*constraint);
           }