Add intensity cost function to the Ceres scan matcher. (#1761)

Not yet used.

Signed-off-by: Wolfgang Hess <whess@lyft.com>

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -86,9 +86,11 @@ std::unique_ptr<transform::Rigid3d> LocalTrajectoryBuilder3D::ScanMatch(
       (matching_submap->local_pose().inverse() * pose_prediction).translation(),
       initial_ceres_pose,
       {{&high_resolution_point_cloud_in_tracking,
-        &matching_submap->high_resolution_hybrid_grid()},
+        &matching_submap->high_resolution_hybrid_grid(),
+        /*intensity_hybrid_grid=*/nullptr},
        {&low_resolution_point_cloud_in_tracking,
-        &matching_submap->low_resolution_hybrid_grid()}},
+        &matching_submap->low_resolution_hybrid_grid(),
+        /*intensity_hybrid_grid=*/nullptr}},
       &pose_observation_in_submap, &summary);
   kCeresScanMatcherCostMetric->Observe(summary.final_cost);
   const double residual_distance = (pose_observation_in_submap.translation() -

cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.cc

@@ -23,6 +23,7 @@
 #include "absl/memory/memory.h"
 #include "cartographer/common/ceres_solver_options.h"
 #include "cartographer/mapping/internal/3d/rotation_parameterization.h"
+#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/occupied_space_cost_function_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h"
 #include "cartographer/mapping/internal/3d/scan_matching/translation_delta_cost_functor_3d.h"
@@ -48,6 +49,24 @@ proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
     options.add_occupied_space_weight(
         parameter_dictionary->GetDouble(lua_identifier));
   }
+  for (int i = 0;; ++i) {
+    const std::string lua_identifier =
+        "intensity_cost_function_options_" + std::to_string(i);
+    if (!parameter_dictionary->HasKey(lua_identifier)) {
+      break;
+    }
+    const auto intensity_cost_function_options_dictionary =
+        parameter_dictionary->GetDictionary(lua_identifier);
+    auto* intensity_cost_function_options =
+        options.add_intensity_cost_function_options();
+    intensity_cost_function_options->set_weight(
+        intensity_cost_function_options_dictionary->GetDouble("weight"));
+    intensity_cost_function_options->set_huber_scale(
+        intensity_cost_function_options_dictionary->GetDouble("huber_scale"));
+    intensity_cost_function_options->set_intensity_threshold(
+        intensity_cost_function_options_dictionary->GetDouble(
+            "intensity_threshold"));
+  }
   options.set_translation_weight(
       parameter_dictionary->GetDouble("translation_weight"));
   options.set_rotation_weight(
@@ -71,10 +90,10 @@ CeresScanMatcher3D::CeresScanMatcher3D(
 void CeresScanMatcher3D::Match(
     const Eigen::Vector3d& target_translation,
     const transform::Rigid3d& initial_pose_estimate,
-    const std::vector<PointCloudAndHybridGridPointers>&
+    const std::vector<PointCloudAndHybridGridsPointers>&
         point_clouds_and_hybrid_grids,
     transform::Rigid3d* const pose_estimate,
-    ceres::Solver::Summary* const summary) {
+    ceres::Solver::Summary* const summary) const {
   ceres::Problem problem;
   optimization::CeresPose ceres_pose(
       initial_pose_estimate, nullptr /* translation_parameterization */,
@@ -91,16 +110,35 @@ void CeresScanMatcher3D::Match(
   for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) {
     CHECK_GT(options_.occupied_space_weight(i), 0.);
     const sensor::PointCloud& point_cloud =
-        *point_clouds_and_hybrid_grids[i].first;
-    const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
+        *point_clouds_and_hybrid_grids[i].point_cloud;
+    const HybridGrid& hybrid_grid =
+        *point_clouds_and_hybrid_grids[i].hybrid_grid;
     problem.AddResidualBlock(
         OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction(
             options_.occupied_space_weight(i) /
                 std::sqrt(static_cast<double>(point_cloud.size())),
             point_cloud, hybrid_grid),
         nullptr /* loss function */, ceres_pose.translation(),
         ceres_pose.rotation());
+    if (point_clouds_and_hybrid_grids[i].intensity_hybrid_grid) {
+      CHECK_GT(options_.intensity_cost_function_options(i).huber_scale(), 0.);
+      CHECK_GT(options_.intensity_cost_function_options(i).weight(), 0.);
+      CHECK_GT(
+          options_.intensity_cost_function_options(i).intensity_threshold(), 0);
+      const IntensityHybridGrid& intensity_hybrid_grid =
+          *point_clouds_and_hybrid_grids[i].intensity_hybrid_grid;
+      problem.AddResidualBlock(
+          IntensityCostFunction3D::CreateAutoDiffCostFunction(
+              options_.intensity_cost_function_options(i).weight() /
+                  std::sqrt(static_cast<double>(point_cloud.size())),
+              options_.intensity_cost_function_options(i).intensity_threshold(),
+              point_cloud, intensity_hybrid_grid),
+          new ceres::HuberLoss(
+              options_.intensity_cost_function_options(i).huber_scale()),
+          ceres_pose.translation(), ceres_pose.rotation());
+    }
   }
+
   CHECK_GT(options_.translation_weight(), 0.);
   problem.AddResidualBlock(
       TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(

cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d.h

@@ -34,8 +34,11 @@ namespace scan_matching {
 proto::CeresScanMatcherOptions3D CreateCeresScanMatcherOptions3D(
     common::LuaParameterDictionary* parameter_dictionary);
 
-using PointCloudAndHybridGridPointers =
-    std::pair<const sensor::PointCloud*, const HybridGrid*>;
+struct PointCloudAndHybridGridsPointers {
+  const sensor::PointCloud* point_cloud;
+  const HybridGrid* hybrid_grid;
+  const IntensityHybridGrid* intensity_hybrid_grid;  // optional
+};
 
 // This scan matcher uses Ceres to align scans with an existing map.
 class CeresScanMatcher3D {
@@ -50,10 +53,10 @@ class CeresScanMatcher3D {
   // 'summary'.
   void Match(const Eigen::Vector3d& target_translation,
              const transform::Rigid3d& initial_pose_estimate,
-             const std::vector<PointCloudAndHybridGridPointers>&
+             const std::vector<PointCloudAndHybridGridsPointers>&
                  point_clouds_and_hybrid_grids,
              transform::Rigid3d* pose_estimate,
-             ceres::Solver::Summary* summary);
+             ceres::Solver::Summary* summary) const;
 
  private:
   const proto::CeresScanMatcherOptions3D options_;

cartographer/mapping/internal/3d/scan_matching/ceres_scan_matcher_3d_test.cc

@@ -35,21 +35,35 @@ class CeresScanMatcher3DTest : public ::testing::Test {
  protected:
   CeresScanMatcher3DTest()
       : hybrid_grid_(1.f),
+        intensity_hybrid_grid_(1.f),
         expected_pose_(
             transform::Rigid3d::Translation(Eigen::Vector3d(-1., 0., 0.))) {
+    std::vector<sensor::RangefinderPoint> points;
+    std::vector<float> intensities;
     for (const Eigen::Vector3f& point :
          {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
           Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
           Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
           Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
-      point_cloud_.push_back({point});
+      points.push_back({point});
+      intensities.push_back(50);
       hybrid_grid_.SetProbability(
           hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() * point), 1.);
+      intensity_hybrid_grid_.AddIntensity(
+          intensity_hybrid_grid_.GetCellIndex(expected_pose_.cast<float>() *
+                                              point),
+          50);
     }
+    point_cloud_ = sensor::PointCloud(points, intensities);
 
     auto parameter_dictionary = common::MakeDictionary(R"text(
         return {
           occupied_space_weight_0 = 1.,
+          intensity_cost_function_options_0 = {
+            weight = 0.5,
+            huber_scale = 55,
+            intensity_threshold = 100,
+          },
           translation_weight = 0.01,
           rotation_weight = 0.1,
           only_optimize_yaw = false,
@@ -67,14 +81,20 @@ class CeresScanMatcher3DTest : public ::testing::Test {
     transform::Rigid3d pose;
 
     ceres::Solver::Summary summary;
-    ceres_scan_matcher_->Match(initial_pose.translation(), initial_pose,
-                               {{&point_cloud_, &hybrid_grid_}}, &pose,
-                               &summary);
+
+    IntensityHybridGrid* intensity_hybrid_grid_ptr =
+        point_cloud_.intensities().empty() ? nullptr : &intensity_hybrid_grid_;
+
+    ceres_scan_matcher_->Match(
+        initial_pose.translation(), initial_pose,
+        {{&point_cloud_, &hybrid_grid_, intensity_hybrid_grid_ptr}}, &pose,
+        &summary);
     EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
     EXPECT_THAT(pose, transform::IsNearly(expected_pose_, 3e-2));
   }
 
   HybridGrid hybrid_grid_;
+  IntensityHybridGrid intensity_hybrid_grid_;
   transform::Rigid3d expected_pose_;
   sensor::PointCloud point_cloud_;
   proto::CeresScanMatcherOptions3D options_;

cartographer/mapping/internal/constraints/constraint_builder_3d.cc

@@ -267,9 +267,11 @@ void ConstraintBuilder3D::ComputeConstraint(
   ceres_scan_matcher_.Match(match_result->pose_estimate.translation(),
                             match_result->pose_estimate,
                             {{&constant_data->high_resolution_point_cloud,
-                              submap_scan_matcher.high_resolution_hybrid_grid},
+                              submap_scan_matcher.high_resolution_hybrid_grid,
+                              /*intensity_hybrid_grid=*/nullptr},
                              {&constant_data->low_resolution_point_cloud,
-                              submap_scan_matcher.low_resolution_hybrid_grid}},
+                              submap_scan_matcher.low_resolution_hybrid_grid,
+                              /*intensity_hybrid_grid=*/nullptr}},
                             &constraint_transform, &unused_summary);
 
   constraint->reset(new Constraint{

cartographer/mapping/proto/scan_matching/ceres_scan_matcher_options_3d.proto

@@ -18,9 +18,16 @@ package cartographer.mapping.scan_matching.proto;
 
 import "cartographer/common/proto/ceres_solver_options.proto";
 
-// NEXT ID: 7
+message IntensityCostFunctionOptions {
+  double weight = 1;
+  double huber_scale = 2;
+  // Ignore ranges with intensity above this threshold.
+  float intensity_threshold = 3;
+}
+
+// NEXT ID: 8
 message CeresScanMatcherOptions3D {
-  // Scaling parameters for each cost functor.
+  // Scaling parameters for each occupied space cost functor.
   repeated double occupied_space_weight = 1;
   double translation_weight = 2;
   double rotation_weight = 3;
@@ -31,4 +38,7 @@ message CeresScanMatcherOptions3D {
   // Configure the Ceres solver. See the Ceres documentation for more
   // information: https://code.google.com/p/ceres-solver/
   common.proto.CeresSolverOptions ceres_solver_options = 6;
+
+  // Scaling parameters for each intensity cost functor.
+  repeated IntensityCostFunctionOptions intensity_cost_function_options = 7;
 }