Move occupied space cost function to .cc (#1200)

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

@@ -71,7 +71,7 @@ void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
   ceres::Problem problem;
   CHECK_GT(options_.occupied_space_weight(), 0.);
   problem.AddResidualBlock(
-      OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
+      CreateOccupiedSpaceCostFunction2D(
           options_.occupied_space_weight() /
               std::sqrt(static_cast<double>(point_cloud.size())),
           point_cloud, grid),

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

@@ -0,0 +1,119 @@
+/*
+ * 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/occupied_space_cost_function_2d.h"
+#include "cartographer/mapping/probability_values.h"
+#include "ceres/cubic_interpolation.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+namespace {
+
+// Computes a cost for matching the 'point_cloud' to the 'grid' with
+// a 'pose'. The cost increases with poorer correspondence of the grid and the
+// point observation (e.g. points falling into less occupied space).
+class OccupiedSpaceCostFunction2D {
+ public:
+  OccupiedSpaceCostFunction2D(const double scaling_factor,
+                              const sensor::PointCloud& point_cloud,
+                              const Grid2D& grid)
+      : scaling_factor_(scaling_factor),
+        point_cloud_(point_cloud),
+        grid_(grid) {}
+
+  template <typename T>
+  bool operator()(const T* const pose, T* residual) const {
+    Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
+    Eigen::Rotation2D<T> rotation(pose[2]);
+    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.);
+
+    const GridArrayAdapter adapter(grid_);
+    ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
+    const MapLimits& limits = grid_.limits();
+
+    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;
+      interpolator.Evaluate(
+          (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
+              static_cast<double>(kPadding),
+          (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
+              static_cast<double>(kPadding),
+          &residual[i]);
+      residual[i] = scaling_factor_ * residual[i];
+    }
+    return true;
+  }
+
+ private:
+  static constexpr int kPadding = INT_MAX / 4;
+  class GridArrayAdapter {
+   public:
+    enum { DATA_DIMENSION = 1 };
+
+    explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {}
+
+    void GetValue(const int row, const int column, double* const value) const {
+      if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
+          column >= NumCols() - kPadding) {
+        *value = kMaxCorrespondenceCost;
+      } else {
+        *value = static_cast<double>(grid_.GetCorrespondenceCost(
+            Eigen::Array2i(column - kPadding, row - kPadding)));
+      }
+    }
+
+    int NumRows() const {
+      return grid_.limits().cell_limits().num_y_cells + 2 * kPadding;
+    }
+
+    int NumCols() const {
+      return grid_.limits().cell_limits().num_x_cells + 2 * kPadding;
+    }
+
+   private:
+    const Grid2D& grid_;
+  };
+
+  OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
+  OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
+      delete;
+
+  const double scaling_factor_;
+  const sensor::PointCloud& point_cloud_;
+  const Grid2D& grid_;
+};
+
+}  // namespace
+
+ceres::CostFunction* CreateOccupiedSpaceCostFunction2D(
+    const double scaling_factor, const sensor::PointCloud& point_cloud,
+    const Grid2D& grid) {
+  return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
+                                         ceres::DYNAMIC /* residuals */,
+                                         3 /* pose variables */>(
+      new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
+      point_cloud.size());
+}
+
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer

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

@@ -17,106 +17,20 @@
 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_
 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_
 
-#include "Eigen/Core"
-#include "Eigen/Geometry"
 #include "cartographer/mapping/2d/grid_2d.h"
-#include "cartographer/mapping/probability_values.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "ceres/ceres.h"
-#include "ceres/cubic_interpolation.h"
 
 namespace cartographer {
 namespace mapping {
 namespace scan_matching {
 
-// Computes a cost for matching the 'point_cloud' to the 'grid' with
+// Creates a cost function for matching the 'point_cloud' to the 'grid' with
 // a 'pose'. The cost increases with poorer correspondence of the grid and the
 // point observation (e.g. points falling into less occupied space).
-class OccupiedSpaceCostFunction2D {
- public:
-  static ceres::CostFunction* CreateAutoDiffCostFunction(
+ceres::CostFunction* CreateOccupiedSpaceCostFunction2D(
     const double scaling_factor, const sensor::PointCloud& point_cloud,
-      const Grid2D& grid) {
-    return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
-                                           ceres::DYNAMIC /* residuals */,
-                                           3 /* pose variables */>(
-        new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
-        point_cloud.size());
-  }
-
-  template <typename T>
-  bool operator()(const T* const pose, T* residual) const {
-    Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
-    Eigen::Rotation2D<T> rotation(pose[2]);
-    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.);
-
-    const GridArrayAdapter adapter(grid_);
-    ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
-    const MapLimits& limits = grid_.limits();
-
-    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;
-      interpolator.Evaluate(
-          (limits.max().x() - world[0]) / limits.resolution() - 0.5 +
-              static_cast<double>(kPadding),
-          (limits.max().y() - world[1]) / limits.resolution() - 0.5 +
-              static_cast<double>(kPadding),
-          &residual[i]);
-      residual[i] = scaling_factor_ * residual[i];
-    }
-    return true;
-  }
-
- private:
-  static constexpr int kPadding = INT_MAX / 4;
-  class GridArrayAdapter {
-   public:
-    enum { DATA_DIMENSION = 1 };
-
-    explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {}
-
-    void GetValue(const int row, const int column, double* const value) const {
-      if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
-          column >= NumCols() - kPadding) {
-        *value = kMaxCorrespondenceCost;
-      } else {
-        *value = static_cast<double>(grid_.GetCorrespondenceCost(
-            Eigen::Array2i(column - kPadding, row - kPadding)));
-      }
-    }
-
-    int NumRows() const {
-      return grid_.limits().cell_limits().num_y_cells + 2 * kPadding;
-    }
-
-    int NumCols() const {
-      return grid_.limits().cell_limits().num_x_cells + 2 * kPadding;
-    }
-
-   private:
-    const Grid2D& grid_;
-  };
-
-  OccupiedSpaceCostFunction2D(const double scaling_factor,
-                              const sensor::PointCloud& point_cloud,
-                              const Grid2D& grid)
-      : scaling_factor_(scaling_factor),
-        point_cloud_(point_cloud),
-        grid_(grid) {}
-
-  OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
-  OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
-      delete;
-
-  const double scaling_factor_;
-  const sensor::PointCloud& point_cloud_;
-  const Grid2D& grid_;
-};
+    const Grid2D& grid);
 
 }  // namespace scan_matching
 }  // namespace mapping

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

@@ -0,0 +1,57 @@
+/*
+ * 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/occupied_space_cost_function_2d.h"
+
+#include "cartographer/mapping/2d/probability_grid.h"
+#include "cartographer/mapping/probability_values.h"
+
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace scan_matching {
+namespace {
+
+using ::testing::DoubleEq;
+using ::testing::ElementsAre;
+
+TEST(OccupiedSpaceCostFunction2DTest, SmokeTest) {
+  ProbabilityGrid grid(
+      MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
+  sensor::PointCloud point_cloud = {Eigen::Vector3f{0.f, 0.f, 0.f}};
+  ceres::Problem problem;
+  std::unique_ptr<ceres::CostFunction> cost_function(
+      CreateOccupiedSpaceCostFunction2D(1.f, point_cloud, grid));
+
+  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();
+  cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
+                          jacobians_ptrs.data());
+
+  EXPECT_THAT(residuals, ElementsAre(DoubleEq(kMaxProbability)));
+}
+
+}  // namespace
+}  // namespace scan_matching
+}  // namespace mapping
+}  // namespace cartographer