FastCorrelativeScanMatcher outputs unique_ptr. (#643)

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.cc

@@ -156,12 +156,12 @@ FastCorrelativeScanMatcher::FastCorrelativeScanMatcher(
 
 FastCorrelativeScanMatcher::~FastCorrelativeScanMatcher() {}
 
-bool FastCorrelativeScanMatcher::Match(
+std::unique_ptr<FastCorrelativeScanMatcher::Result>
+FastCorrelativeScanMatcher::Match(
     const transform::Rigid3d& global_node_pose,
     const transform::Rigid3d& global_submap_pose,
-    const mapping::TrajectoryNode::Data& constant_data, const float min_score,
+    const mapping::TrajectoryNode::Data& constant_data,
-    float* const score, transform::Rigid3d* const pose_estimate,
+    const float min_score) const {
-    float* const rotational_score, float* const low_resolution_score) const {
   const auto low_resolution_matcher = scan_matching::CreateLowResolutionMatcher(
       low_resolution_hybrid_grid_, &constant_data.low_resolution_point_cloud);
   const SearchParameters search_parameters{
@@ -173,16 +173,15 @@ bool FastCorrelativeScanMatcher::Match(
       global_submap_pose.cast<float>(),
       constant_data.high_resolution_point_cloud,
       constant_data.rotational_scan_matcher_histogram,
-      constant_data.gravity_alignment, min_score, score, pose_estimate,
+      constant_data.gravity_alignment, min_score);
-      rotational_score, low_resolution_score);
 }
 
-bool FastCorrelativeScanMatcher::MatchFullSubmap(
+std::unique_ptr<FastCorrelativeScanMatcher::Result>
+FastCorrelativeScanMatcher::MatchFullSubmap(
     const Eigen::Quaterniond& global_node_rotation,
     const Eigen::Quaterniond& global_submap_rotation,
-    const mapping::TrajectoryNode::Data& constant_data, const float min_score,
+    const mapping::TrajectoryNode::Data& constant_data,
-    float* const score, transform::Rigid3d* const pose_estimate,
+    const float min_score) const {
-    float* const rotational_score, float* const low_resolution_score) const {
   float max_point_distance = 0.f;
   for (const Eigen::Vector3f& point :
        constant_data.high_resolution_point_cloud) {
@@ -201,22 +200,17 @@ bool FastCorrelativeScanMatcher::MatchFullSubmap(
       transform::Rigid3f::Rotation(global_submap_rotation.cast<float>()),
       constant_data.high_resolution_point_cloud,
       constant_data.rotational_scan_matcher_histogram,
-      constant_data.gravity_alignment, min_score, score, pose_estimate,
+      constant_data.gravity_alignment, min_score);
-      rotational_score, low_resolution_score);
 }
 
-bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
+std::unique_ptr<FastCorrelativeScanMatcher::Result>
+FastCorrelativeScanMatcher::MatchWithSearchParameters(
     const FastCorrelativeScanMatcher::SearchParameters& search_parameters,
     const transform::Rigid3f& global_node_pose,
     const transform::Rigid3f& global_submap_pose,
     const sensor::PointCloud& point_cloud,
     const Eigen::VectorXf& rotational_scan_matcher_histogram,
-    const Eigen::Quaterniond& gravity_alignment, const float min_score,
+    const Eigen::Quaterniond& gravity_alignment, const float min_score) const {
-    float* const score, transform::Rigid3d* const pose_estimate,
-    float* const rotational_score, float* const low_resolution_score) const {
-  CHECK_NOTNULL(score);
-  CHECK_NOTNULL(pose_estimate);
-
   const std::vector<DiscreteScan> discrete_scans = GenerateDiscreteScans(
       search_parameters, point_cloud, rotational_scan_matcher_histogram,
       gravity_alignment, global_node_pose, global_submap_pose);
@@ -228,15 +222,13 @@ bool FastCorrelativeScanMatcher::MatchWithSearchParameters(
       search_parameters, discrete_scans, lowest_resolution_candidates,
       precomputation_grid_stack_->max_depth(), min_score);
   if (best_candidate.score > min_score) {
-    *score = best_candidate.score;
+    return common::make_unique<Result>(Result{
-    *pose_estimate =
+        best_candidate.score,
-        GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>();
+        GetPoseFromCandidate(discrete_scans, best_candidate).cast<double>(),
-    *rotational_score =
+        discrete_scans[best_candidate.scan_index].rotational_score,
-        discrete_scans[best_candidate.scan_index].rotational_score;
+        best_candidate.low_resolution_score});
-    *low_resolution_score = best_candidate.low_resolution_score;
-    return true;
   }
-  return false;
+  return nullptr;
 }
 
 DiscreteScan FastCorrelativeScanMatcher::DiscretizeScan(

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher.h

@@ -49,6 +49,13 @@ using MatchingFunction = std::function<float(const transform::Rigid3f&)>;
 
 class FastCorrelativeScanMatcher {
  public:
+  struct Result {
+    float score;
+    transform::Rigid3d pose_estimate;
+    float rotational_score;
+    float low_resolution_score;
+  };
+
   FastCorrelativeScanMatcher(
       const HybridGrid& hybrid_grid,
       const HybridGrid* low_resolution_hybrid_grid,
@@ -61,28 +68,23 @@ class FastCorrelativeScanMatcher {
       delete;
 
   // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
-  // given 'global_node_pose' and 'global_submap_pose'. If a score above
+  // given 'global_node_pose' and 'global_submap_pose'. 'Result' is only
-  // 'min_score' (excluding equality) is possible, true is returned, and
+  // returned if a score above 'min_score' (excluding equality) is possible.
-  // 'score', 'pose_estimate', 'rotational_score', and 'low_resolution_score'
+  std::unique_ptr<Result> Match(
-  // are updated with the result.
+      const transform::Rigid3d& global_node_pose,
-  bool Match(const transform::Rigid3d& global_node_pose,
+      const transform::Rigid3d& global_submap_pose,
-             const transform::Rigid3d& global_submap_pose,
+      const mapping::TrajectoryNode::Data& constant_data,
-             const mapping::TrajectoryNode::Data& constant_data,
+      float min_score) const;
-             float min_score, float* score, transform::Rigid3d* pose_estimate,
-             float* rotational_score, float* low_resolution_score) const;
 
   // Aligns the node with the given 'constant_data' within the 'hybrid_grid'
   // given rotations which are expected to be approximately gravity aligned.
-  // If a score above 'min_score' (excluding equality) is possible, true is
+  // 'Result' is only returned if a score above 'min_score' (excluding equality)
-  // returned, and 'score', 'pose_estimate', 'rotational_score', and
+  // is possible.
-  // 'low_resolution_score' are updated with the result.
+  std::unique_ptr<Result> MatchFullSubmap(
-  bool MatchFullSubmap(const Eigen::Quaterniond& global_node_rotation,
+      const Eigen::Quaterniond& global_node_rotation,
-                       const Eigen::Quaterniond& global_submap_rotation,
+      const Eigen::Quaterniond& global_submap_rotation,
-                       const mapping::TrajectoryNode::Data& constant_data,
+      const mapping::TrajectoryNode::Data& constant_data,
-                       float min_score, float* score,
+      float min_score) const;
-                       transform::Rigid3d* pose_estimate,
-                       float* rotational_score,
-                       float* low_resolution_score) const;
 
  private:
   struct SearchParameters {
@@ -92,15 +94,13 @@ class FastCorrelativeScanMatcher {
     const MatchingFunction* const low_resolution_matcher;
   };
 
-  bool MatchWithSearchParameters(
+  std::unique_ptr<Result> MatchWithSearchParameters(
       const SearchParameters& search_parameters,
       const transform::Rigid3f& global_node_pose,
       const transform::Rigid3f& global_submap_pose,
       const sensor::PointCloud& point_cloud,
       const Eigen::VectorXf& rotational_scan_matcher_histogram,
-      const Eigen::Quaterniond& gravity_alignment, float min_score,
+      const Eigen::Quaterniond& gravity_alignment, float min_score) const;
-      float* score, transform::Rigid3d* pose_estimate, float* rotational_score,
-      float* low_resolution_score) const;
   DiscreteScan DiscretizeScan(const SearchParameters& search_parameters,
                               const sensor::PointCloud& point_cloud,
                               const transform::Rigid3f& pose,

cartographer/mapping_3d/scan_matching/fast_correlative_scan_matcher_test.cc

@@ -140,26 +140,25 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatch) {
     std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
         GetFastCorrelativeScanMatcher(options_, expected_pose));
 
-    float score = 0.f;
+    const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
-    transform::Rigid3d pose_estimate;
+        fast_correlative_scan_matcher->Match(
-    float rotational_score = 0.f;
+            transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
-    float low_resolution_score = 0.f;
+            CreateConstantData(point_cloud_), kMinScore);
-    EXPECT_TRUE(fast_correlative_scan_matcher->Match(
+    EXPECT_THAT(result, testing::NotNull());
-        transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
+    EXPECT_LT(kMinScore, result->score);
-        CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
+    EXPECT_LT(0.09f, result->rotational_score);
-        &rotational_score, &low_resolution_score));
+    EXPECT_LT(0.14f, result->low_resolution_score);
-    EXPECT_LT(kMinScore, score);
-    EXPECT_LT(0.09f, rotational_score);
-    EXPECT_LT(0.14f, low_resolution_score);
     EXPECT_THAT(expected_pose,
-                transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
+                transform::IsNearly(result->pose_estimate.cast<float>(), 0.05f))
-        << "Actual: " << transform::ToProto(pose_estimate).DebugString()
+        << "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
         << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
-    EXPECT_FALSE(fast_correlative_scan_matcher->Match(
+
-        transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
+    const std::unique_ptr<FastCorrelativeScanMatcher::Result>
-        CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
+        low_resolution_result = fast_correlative_scan_matcher->Match(
-        &score, &pose_estimate, &rotational_score, &low_resolution_score))
+            transform::Rigid3d::Identity(), transform::Rigid3d::Identity(),
-        << low_resolution_score;
+            CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
+    EXPECT_THAT(low_resolution_result, testing::IsNull())
+        << low_resolution_result->low_resolution_score;
   }
 }
 
@@ -169,26 +168,25 @@ TEST_F(FastCorrelativeScanMatcherTest, CorrectPoseForMatchFullSubmap) {
   std::unique_ptr<FastCorrelativeScanMatcher> fast_correlative_scan_matcher(
       GetFastCorrelativeScanMatcher(options_, expected_pose));
 
-  float score = 0.f;
+  const std::unique_ptr<FastCorrelativeScanMatcher::Result> result =
-  transform::Rigid3d pose_estimate;
+      fast_correlative_scan_matcher->MatchFullSubmap(
-  float rotational_score = 0.f;
+          Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
-  float low_resolution_score = 0.f;
+          CreateConstantData(point_cloud_), kMinScore);
-  EXPECT_TRUE(fast_correlative_scan_matcher->MatchFullSubmap(
+  EXPECT_THAT(result, testing::NotNull());
-      Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
+  EXPECT_LT(kMinScore, result->score);
-      CreateConstantData(point_cloud_), kMinScore, &score, &pose_estimate,
+  EXPECT_LT(0.09f, result->rotational_score);
-      &rotational_score, &low_resolution_score));
+  EXPECT_LT(0.14f, result->low_resolution_score);
-  EXPECT_LT(kMinScore, score);
-  EXPECT_LT(0.09f, rotational_score);
-  EXPECT_LT(0.14f, low_resolution_score);
   EXPECT_THAT(expected_pose,
-              transform::IsNearly(pose_estimate.cast<float>(), 0.05f))
+              transform::IsNearly(result->pose_estimate.cast<float>(), 0.05f))
-      << "Actual: " << transform::ToProto(pose_estimate).DebugString()
+      << "Actual: " << transform::ToProto(result->pose_estimate).DebugString()
       << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
-  EXPECT_FALSE(fast_correlative_scan_matcher->MatchFullSubmap(
+
-      Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
+  const std::unique_ptr<FastCorrelativeScanMatcher::Result>
-      CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore,
+      low_resolution_result = fast_correlative_scan_matcher->MatchFullSubmap(
-      &score, &pose_estimate, &rotational_score, &low_resolution_score))
+          Eigen::Quaterniond::Identity(), Eigen::Quaterniond::Identity(),
-      << low_resolution_score;
+          CreateConstantData({Eigen::Vector3f(42.f, 42.f, 42.f)}), kMinScore);
+  EXPECT_THAT(low_resolution_result, testing::IsNull())
+      << low_resolution_result->low_resolution_score;
 }
 
 }  // namespace

cartographer/mapping_3d/sparse_pose_graph/constraint_builder.cc

@@ -179,43 +179,41 @@ void ConstraintBuilder::ComputeConstraint(
   // 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).
-  float score = 0.f;
+  std::unique_ptr<scan_matching::FastCorrelativeScanMatcher::Result>
-  transform::Rigid3d pose_estimate;
+      match_result;
-  float rotational_score = 0.f;
-  float low_resolution_score = 0.f;
-  // TODO(gaschler): Match methods should return unique_ptr<struct>.
 
   // 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(
+    match_result =
+        submap_scan_matcher->fast_correlative_scan_matcher->MatchFullSubmap(
             global_node_pose.rotation(), global_submap_pose.rotation(),
-            *constant_data, options_.global_localization_min_score(), &score,
+            *constant_data, options_.global_localization_min_score());
-            &pose_estimate, &rotational_score, &low_resolution_score)) {
+    if (match_result != nullptr) {
-      CHECK_GT(score, options_.global_localization_min_score());
+      CHECK_GT(match_result->score, options_.global_localization_min_score());
       CHECK_GE(node_id.trajectory_id, 0);
       CHECK_GE(submap_id.trajectory_id, 0);
     } else {
       return;
     }
   } else {
-    if (submap_scan_matcher->fast_correlative_scan_matcher->Match(
+    match_result = submap_scan_matcher->fast_correlative_scan_matcher->Match(
-            global_node_pose, global_submap_pose, *constant_data,
+        global_node_pose, global_submap_pose, *constant_data,
-            options_.min_score(), &score, &pose_estimate, &rotational_score,
+        options_.min_score());
-            &low_resolution_score)) {
+    if (match_result != nullptr) {
       // We've reported a successful local match.
-      CHECK_GT(score, options_.min_score());
+      CHECK_GT(match_result->score, options_.min_score());
     } else {
       return;
     }
   }
   {
     common::MutexLocker locker(&mutex_);
-    score_histogram_.Add(score);
+    score_histogram_.Add(match_result->score);
-    rotational_score_histogram_.Add(rotational_score);
+    rotational_score_histogram_.Add(match_result->rotational_score);
-    low_resolution_score_histogram_.Add(low_resolution_score);
+    low_resolution_score_histogram_.Add(match_result->low_resolution_score);
   }
 
   // Use the CSM estimate as both the initial and previous pose. This has the
@@ -223,7 +221,8 @@ void ConstraintBuilder::ComputeConstraint(
   // CSM estimate.
   ceres::Solver::Summary unused_summary;
   transform::Rigid3d constraint_transform;
-  ceres_scan_matcher_.Match(pose_estimate, pose_estimate,
+  ceres_scan_matcher_.Match(match_result->pose_estimate,
+                            match_result->pose_estimate,
                             {{&constant_data->high_resolution_point_cloud,
                               submap_scan_matcher->high_resolution_hybrid_grid},
                              {&constant_data->low_resolution_point_cloud,
@@ -252,7 +251,8 @@ void ConstraintBuilder::ComputeConstraint(
            << difference.translation().norm() << " rotation "
            << std::setprecision(3) << transform::GetAngle(difference);
     }
-    info << " with score " << std::setprecision(1) << 100. * score << "%.";
+    info << " with score " << std::setprecision(1) << 100. * match_result->score
+         << "%.";
     LOG(INFO) << info.str();
   }
 }