Add TSDF match cost function. (#1359)

cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h

@@ -96,11 +96,11 @@ class InterpolatedTSDF2D {
   T InterpolateBilinear(const T& x, const T& y, float x1, float y1, float x2,
                         float y2, float q11, float q12, float q21,
                         float q22) const {
-    const T normalized_x = (x - x1) / (x2 - x1);
-    const T normalized_y = (y - y1) / (y2 - y1);
-    const T q1 = (q12 - q11) * normalized_y + q11;
-    const T q2 = (q22 - q21) * normalized_y + q21;
-    return (q2 - q1) * normalized_x + q1;
+    const T normalized_x = (x - T(x1)) / T(x2 - x1);
+    const T normalized_y = (y - T(y1)) / T(y2 - y1);
+    const T q1 = T(q12 - q11) * normalized_y + T(q11);
+    const T q2 = T(q22 - q21) * normalized_y + T(q21);
+    return T(q2 - q1) * normalized_x + T(q1);
   }
 
   // Center of the next lower pixel, i.e., not necessarily the pixel containing

cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.cc

@@ -0,0 +1,88 @@
+/*
+ * Copyright 2018 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 "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/mapping/internal/2d/scan_matching/interpolated_tsdf_2d.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "ceres/ceres.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+
+// Computes a cost for matching the 'point_cloud' in the 'grid' at
+// a 'pose'. The cost increases with the signed distance of the matched point
+// location in the 'grid'.
+class TSDFMatchCostFunction2D {
+ public:
+  TSDFMatchCostFunction2D(const double residual_scaling_factor,
+                          const sensor::PointCloud& point_cloud,
+                          const TSDF2D& grid)
+      : residual_scaling_factor_(residual_scaling_factor),
+        point_cloud_(point_cloud),
+        interpolated_grid_(grid) {}
+
+  template <typename T>
+  bool operator()(const T* const pose, T* residual) const {
+    const Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
+    const Eigen::Rotation2D<T> rotation(pose[2]);
+    const Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
+    Eigen::Matrix<T, 3, 3> transform;
+    transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
+
+    T summed_weight = T(0);
+    for (size_t i = 0; i < point_cloud_.size(); ++i) {
+      // Note that this is a 2D point. The third component is a scaling factor.
+      const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
+                                         (T(point_cloud_[i].y())), T(1.));
+      const Eigen::Matrix<T, 3, 1> world = transform * point;
+      const T point_weight = interpolated_grid_.GetWeight(world[0], world[1]);
+      summed_weight += point_weight;
+      residual[i] =
+          T(point_cloud_.size()) * residual_scaling_factor_ *
+          interpolated_grid_.GetCorrespondenceCost(world[0], world[1]) *
+          point_weight;
+    }
+    if (summed_weight == T(0)) return false;
+    for (size_t i = 0; i < point_cloud_.size(); ++i) {
+      residual[i] /= summed_weight;
+    }
+    return true;
+  }
+
+ private:
+  TSDFMatchCostFunction2D(const TSDFMatchCostFunction2D&) = delete;
+  TSDFMatchCostFunction2D& operator=(const TSDFMatchCostFunction2D&) = delete;
+
+  const double residual_scaling_factor_;
+  const sensor::PointCloud& point_cloud_;
+  const InterpolatedTSDF2D interpolated_grid_;
+};
+
+ceres::CostFunction* CreateTSDFMatchCostFunction2D(
+    const double scaling_factor, const sensor::PointCloud& point_cloud,
+    const TSDF2D& tsdf) {
+  return new ceres::AutoDiffCostFunction<TSDFMatchCostFunction2D,
+                                         ceres::DYNAMIC /* residuals */,
+                                         3 /* pose variables */>(
+      new TSDFMatchCostFunction2D(scaling_factor, point_cloud, tsdf),
+      point_cloud.size());
+}
+
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d.h

@@ -0,0 +1,39 @@
+/*
+ * Copyright 2018 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_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_
+#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_
+
+#include "cartographer/mapping/2d/tsdf_2d.h"
+#include "cartographer/sensor/point_cloud.h"
+#include "ceres/ceres.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+
+// Creates a cost function for matching the 'point_cloud' in the 'grid' at
+// a 'pose'. The cost increases with the signed distance of the matched point
+// location in the 'grid'.
+ceres::CostFunction* CreateTSDFMatchCostFunction2D(
+    const double scaling_factor, const sensor::PointCloud& point_cloud,
+    const TSDF2D& grid);
+
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer
+
+#endif  // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TSDF_MATCH_COST_FUNCTION_2D_H_

cartographer/mapping/internal/2d/scan_matching/tsdf_match_cost_function_2d_test.cc

@@ -0,0 +1,167 @@
+/*
+ * Copyright 2018 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/2d/scan_matching/tsdf_match_cost_function_2d.h"
+
+#include "cartographer/common/lua_parameter_dictionary.h"
+#include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
+#include "cartographer/mapping/2d/tsdf_2d.h"
+#include "cartographer/mapping/2d/tsdf_range_data_inserter_2d.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+namespace {
+
+using ::testing::DoubleNear;
+using ::testing::ElementsAre;
+
+class TSDFSpaceCostFunction2DTest : public ::testing::Test {
+ protected:
+  TSDFSpaceCostFunction2DTest()
+      : tsdf_(MapLimits(0.1, Eigen::Vector2d(2.05, 2.05), CellLimits(40, 40)),
+              0.3, 1.0, &conversion_tables_) {
+    auto parameter_dictionary = common::MakeDictionary(
+        "return { "
+        "truncation_distance = 0.3,"
+        "maximum_weight = 1.0,"
+        "update_free_space = false,"
+        "normal_estimation_options = {"
+        "num_normal_samples = 2,"
+        "sample_radius = 10.,"
+        "},"
+        "project_sdf_distance_to_scan_normal = true,"
+        "update_weight_range_exponent = 0,"
+        "update_weight_angle_scan_normal_to_ray_kernel_bandwith = 0,"
+        "update_weight_distance_cell_to_hit_kernel_bandwith = 0,"
+        "}");
+    options_ = CreateTSDFRangeDataInserterOptions2D(parameter_dictionary.get());
+    range_data_inserter_ = absl::make_unique<TSDFRangeDataInserter2D>(options_);
+  }
+
+  void InsertPointcloud() {
+    sensor::RangeData range_data;
+    for (float x = -.5; x < 0.5f; x += 0.1) {
+      range_data.returns.emplace_back(x, 1.0f, 0.f);
+    }
+    range_data.origin.x() = -0.5f;
+    range_data.origin.y() = -0.5f;
+    range_data_inserter_->Insert(range_data, &tsdf_);
+    tsdf_.FinishUpdate();
+  }
+
+  ValueConversionTables conversion_tables_;
+  proto::TSDFRangeDataInserterOptions2D options_;
+  TSDF2D tsdf_;
+  std::unique_ptr<TSDFRangeDataInserter2D> range_data_inserter_;
+};
+
+TEST_F(TSDFSpaceCostFunction2DTest, MatchEmptyTSDF) {
+  const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
+  const std::array<double, 3> pose_estimate{{0., 0., 0.}};
+  const std::array<const double*, 1> parameter_blocks{{pose_estimate.data()}};
+  std::array<double, 1> residuals;
+  std::array<std::array<double, 3>, 1> jacobians;
+  std::array<double*, 1> jacobians_ptrs;
+  for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data();
+  bool valid_result = cost_function->Evaluate(
+      parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
+  EXPECT_FALSE(valid_result);
+}
+
+TEST_F(TSDFSpaceCostFunction2DTest, ExactInitialPose) {
+  InsertPointcloud();
+  const sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
+  const std::array<double, 3> pose_estimate{{0., 0., 0.}};
+  const std::array<const double*, 1> parameter_blocks{{pose_estimate.data()}};
+  std::array<double, 1> residuals;
+  std::array<std::array<double, 3>, 1> jacobians;
+  std::array<double*, 1> jacobians_ptrs;
+  for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data();
+  const bool valid_result = cost_function->Evaluate(
+      parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
+  EXPECT_TRUE(valid_result);
+  EXPECT_THAT(residuals, ElementsAre(DoubleNear(0., 1e-3)));
+  EXPECT_THAT(jacobians[0],
+              ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3),
+                          DoubleNear(0., 1e-3)));
+}
+
+TEST_F(TSDFSpaceCostFunction2DTest, PertubatedInitialPose) {
+  InsertPointcloud();
+  sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
+  std::array<double, 3> pose_estimate{{0., 0.1, 0.}};
+  std::array<const double*, 1> parameter_blocks{{pose_estimate.data()}};
+  std::array<double, 1> residuals;
+  std::array<std::array<double, 3>, 1> jacobians;
+  std::array<double*, 1> jacobians_ptrs;
+  for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data();
+
+  bool valid_result = cost_function->Evaluate(
+      parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
+  EXPECT_TRUE(valid_result);
+  EXPECT_THAT(residuals, ElementsAre(DoubleNear(-0.1, 1e-3)));
+  EXPECT_THAT(jacobians[0],
+              ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3),
+                          DoubleNear(0., 1e-3)));
+
+  pose_estimate[1] = -0.1;
+  parameter_blocks = {{pose_estimate.data()}};
+  valid_result = cost_function->Evaluate(
+      parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
+  EXPECT_TRUE(valid_result);
+  EXPECT_THAT(residuals, ElementsAre(DoubleNear(0.1, 1e-3)));
+  EXPECT_THAT(jacobians[0],
+              ElementsAre(DoubleNear(0., 1e-3), DoubleNear(-1., 1e-3),
+                          DoubleNear(0., 1e-3)));
+}
+
+TEST_F(TSDFSpaceCostFunction2DTest, InvalidInitialPose) {
+  InsertPointcloud();
+  sensor::PointCloud matching_cloud = {Eigen::Vector3f{0.f, 1.0f, 0.f}};
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      CreateTSDFMatchCostFunction2D(1.f, matching_cloud, tsdf_));
+  std::array<double, 3> pose_estimate{{0., 0.4, 0.}};
+  std::array<const double*, 1> parameter_blocks{{pose_estimate.data()}};
+  std::array<double, 1> residuals;
+  std::array<std::array<double, 3>, 1> jacobians;
+  std::array<double*, 1> jacobians_ptrs;
+  for (int i = 0; i < 1; ++i) jacobians_ptrs[i] = jacobians[i].data();
+
+  bool valid_result = cost_function->Evaluate(
+      parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
+  EXPECT_FALSE(valid_result);
+
+  pose_estimate[1] = -0.4;
+  parameter_blocks = {{pose_estimate.data()}};
+  valid_result = cost_function->Evaluate(
+      parameter_blocks.data(), residuals.data(), jacobians_ptrs.data());
+  EXPECT_FALSE(valid_result);
+}
+
+}  // namespace
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer