Add a cost function to take intensities into account. (#1760)

Adds (optional) intensities to sensor::PointCloud and
uses it in a cost function. Not yet in use.

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

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

@@ -0,0 +1,25 @@
+#include "cartographer/mapping/internal/3d/scan_matching/intensity_cost_function_3d.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+
+// This method is defined here instead of the header file as it was observed
+// that defining it in the header file has a negative impact on the runtime
+// performance.
+ceres::CostFunction* IntensityCostFunction3D::CreateAutoDiffCostFunction(
+    const double scaling_factor, const float intensity_threshold,
+    const sensor::PointCloud& point_cloud,
+    const IntensityHybridGrid& hybrid_grid) {
+  CHECK(!point_cloud.intensities().empty());
+  return new ceres::AutoDiffCostFunction<
+      IntensityCostFunction3D, ceres::DYNAMIC /* residuals */,
+      3 /* translation variables */, 4 /* rotation variables */>(
+      new IntensityCostFunction3D(scaling_factor, intensity_threshold,
+                                  point_cloud, hybrid_grid),
+      point_cloud.size());
+}
+
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer

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

@@ -0,0 +1,98 @@
+/*
+ * Copyright 2019 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_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_
+
+#include "Eigen/Core"
+#include "cartographer/mapping/3d/hybrid_grid.h"
+#include "cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "ceres/ceres.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+
+// Computes a cost for matching the 'point_cloud' to the 'hybrid_grid' with a
+// 'translation' and 'rotation'. The cost increases when points fall into space
+// for which different intensity has been observed, i.e. at voxels with different
+// values. Only points up to a certain threshold are evaluated which is intended
+// to ignore data from retroreflections.
+class IntensityCostFunction3D {
+ public:
+  static ceres::CostFunction* CreateAutoDiffCostFunction(
+      const double scaling_factor, const float intensity_threshold,
+      const sensor::PointCloud& point_cloud,
+      const IntensityHybridGrid& hybrid_grid);
+
+  template <typename T>
+  bool operator()(const T* const translation, const T* const rotation,
+                  T* const residual) const {
+    const transform::Rigid3<T> transform(
+        Eigen::Map<const Eigen::Matrix<T, 3, 1>>(translation),
+        Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2],
+                             rotation[3]));
+    return Evaluate(transform, residual);
+  }
+
+ private:
+  IntensityCostFunction3D(const double scaling_factor,
+                          const float intensity_threshold,
+                          const sensor::PointCloud& point_cloud,
+                          const IntensityHybridGrid& hybrid_grid)
+      : scaling_factor_(scaling_factor),
+        intensity_threshold_(intensity_threshold),
+        point_cloud_(point_cloud),
+        interpolated_grid_(hybrid_grid) {}
+
+  IntensityCostFunction3D(const IntensityCostFunction3D&) = delete;
+  IntensityCostFunction3D& operator=(const IntensityCostFunction3D&) = delete;
+
+  template <typename T>
+  bool Evaluate(const transform::Rigid3<T>& transform,
+                T* const residual) const {
+    for (size_t i = 0; i < point_cloud_.size(); ++i) {
+      if (point_cloud_.intensities()[i] > intensity_threshold_) {
+        residual[i] = T(0.f);
+      } else {
+        const Eigen::Matrix<T, 3, 1> point = point_cloud_[i].position.cast<T>();
+        const T intensity = T(point_cloud_.intensities()[i]);
+
+        const Eigen::Matrix<T, 3, 1> world = transform * point;
+        const T interpolated_intensity =
+            interpolated_grid_.GetInterpolatedValue(world[0], world[1],
+                                                    world[2]);
+        residual[i] = scaling_factor_ * (interpolated_intensity - intensity);
+      }
+    }
+    return true;
+  }
+
+  const double scaling_factor_;
+  // We will ignore returns with intensity above this threshold.
+  const float intensity_threshold_;
+  const sensor::PointCloud& point_cloud_;
+  const InterpolatedIntensityGrid interpolated_grid_;
+};
+
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_INTENSITY_COST_FUNCTION_3D_H_

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

@@ -0,0 +1,66 @@
+/*
+ * Copyright 2019 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/internal/3d/scan_matching/intensity_cost_function_3d.h"
+
+#include <array>
+#include <memory>
+
+#include "cartographer/mapping/3d/hybrid_grid.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "ceres/ceres.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+namespace {
+
+using ::testing::DoubleNear;
+using ::testing::ElementsAre;
+
+TEST(IntensityCostFunction3DTest, SmokeTest) {
+  const sensor::PointCloud point_cloud(
+      {{{0.f, 0.f, 0.f}}, {{1.f, 1.f, 1.f}}, {{2.f, 2.f, 2.f}}},
+      {50.f, 100.f, 150.f});
+  IntensityHybridGrid hybrid_grid(0.3f);
+  hybrid_grid.AddIntensity(hybrid_grid.GetCellIndex({0.f, 0.f, 0.f}), 50.f);
+
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      IntensityCostFunction3D::CreateAutoDiffCostFunction(
+          /*scaling_factor=*/1.0f, /*intensity_threshold=*/100.f, point_cloud,
+          hybrid_grid));
+
+  const std::array<double, 3> translation{{0., 0., 0.}};
+  const std::array<double, 4> rotation{{1., 0., 0., 0.}};
+
+  const std::array<const double*, 2> parameter_blocks{
+      {translation.data(), rotation.data()}};
+  std::array<double, 3> residuals;
+
+  cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
+                          /*jacobians=*/nullptr);
+  EXPECT_THAT(residuals,
+              ElementsAre(DoubleNear(0., 1e-9), DoubleNear(-100., 1e-9),
+                          DoubleNear(0., 1e-9)));
+}
+
+}  // namespace
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer

cartographer/sensor/point_cloud.cc

@@ -25,6 +25,13 @@ namespace sensor {
 PointCloud::PointCloud() {}
 PointCloud::PointCloud(std::vector<PointCloud::PointType> points)
     : points_(std::move(points)) {}
+PointCloud::PointCloud(std::vector<PointType> points,
+                       std::vector<float> intensities)
+    : points_(std::move(points)), intensities_(std::move(intensities)) {
+  if (!intensities_.empty()) {
+    CHECK_EQ(points_.size(), intensities_.size());
+  }
+}
 
 size_t PointCloud::size() const { return points_.size(); }
 bool PointCloud::empty() const { return points_.empty(); }
@@ -32,6 +39,9 @@ bool PointCloud::empty() const { return points_.empty(); }
 const std::vector<PointCloud::PointType>& PointCloud::points() const {
   return points_;
 }
+const std::vector<float>& PointCloud::intensities() const {
+  return intensities_;
+}
 const PointCloud::PointType& PointCloud::operator[](const size_t index) const {
   return points_[index];
 }

cartographer/sensor/point_cloud.h

@@ -30,12 +30,13 @@ namespace sensor {
 
 // Stores 3D positions of points together with some additional data, e.g.
 // intensities.
-struct PointCloud {
+class PointCloud {
  public:
   using PointType = RangefinderPoint;
 
   PointCloud();
   explicit PointCloud(std::vector<PointType> points);
+  PointCloud(std::vector<PointType> points, std::vector<float> intensities);
 
   // Returns the number of points in the point cloud.
   size_t size() const;
@@ -43,6 +44,7 @@ struct PointCloud {
   bool empty() const;
 
   const std::vector<PointType>& points() const;
+  const std::vector<float>& intensities() const;
   const PointType& operator[](const size_t index) const;
 
   // Iterator over the points in the point cloud.
@@ -55,6 +57,9 @@ struct PointCloud {
  private:
   // For 2D points, the third entry is 0.f.
   std::vector<PointType> points_;
+  // Intensities are optional. If non-empty, they must have the same size as
+  // points.
+  std::vector<float> intensities_;
 };
 
 // Stores 3D positions of points with their relative measurement time in the