Add analytical 2d cost function for pose graph (not used yet). (#1161)

cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc

@@ -0,0 +1,150 @@
+/*
+ * 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/optimization/cost_functions/spa_cost_function_2d.h"
+
+#include <array>
+
+#include "Eigen/Core"
+#include "Eigen/Geometry"
+#include "cartographer/common/math.h"
+#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
+#include "cartographer/transform/rigid_transform.h"
+#include "cartographer/transform/transform.h"
+#include "ceres/jet.h"
+
+namespace cartographer {
+namespace mapping {
+namespace optimization {
+namespace {
+
+class SpaCostFunction2D {
+ public:
+  explicit SpaCostFunction2D(
+      const PoseGraphInterface::Constraint::Pose& observed_relative_pose)
+      : observed_relative_pose_(observed_relative_pose) {}
+
+  template <typename T>
+  bool operator()(const T* const start_pose, const T* const end_pose,
+                  T* e) const {
+    const std::array<T, 3> error =
+        ScaleError(ComputeUnscaledError(
+                       transform::Project2D(observed_relative_pose_.zbar_ij),
+                       start_pose, end_pose),
+                   observed_relative_pose_.translation_weight,
+                   observed_relative_pose_.rotation_weight);
+    std::copy(std::begin(error), std::end(error), e);
+    return true;
+  }
+
+ private:
+  const PoseGraphInterface::Constraint::Pose observed_relative_pose_;
+};
+
+class AnalyticalSpaCostFunction2D
+    : public ceres::SizedCostFunction<3 /* number of residuals */,
+                                      3 /* size of start pose */,
+                                      3 /* size of end pose */> {
+ public:
+  explicit AnalyticalSpaCostFunction2D(
+      const PoseGraphInterface::Constraint::Pose& constraint_pose)
+      : observed_relative_pose_(transform::Project2D(constraint_pose.zbar_ij)),
+        translation_weight_(constraint_pose.translation_weight),
+        rotation_weight_(constraint_pose.rotation_weight) {}
+  virtual ~AnalyticalSpaCostFunction2D() {}
+
+  bool Evaluate(double const* const* parameters, double* residuals,
+                double** jacobians) const override {
+    double const* start = parameters[0];
+    double const* end = parameters[1];
+
+    const double cos_start_rotation = cos(start[2]);
+    const double sin_start_rotation = sin(start[2]);
+    const double delta_x = end[0] - start[0];
+    const double delta_y = end[1] - start[1];
+
+    residuals[0] =
+        translation_weight_ *
+        (observed_relative_pose_.translation().x() -
+         (cos_start_rotation * delta_x + sin_start_rotation * delta_y));
+    residuals[1] =
+        translation_weight_ *
+        (observed_relative_pose_.translation().y() -
+         (-sin_start_rotation * delta_x + cos_start_rotation * delta_y));
+    residuals[2] =
+        rotation_weight_ *
+        common::NormalizeAngleDifference(
+            observed_relative_pose_.rotation().angle() - (end[2] - start[2]));
+    if (jacobians == NULL) return true;
+
+    const double weighted_cos_start_rotation =
+        translation_weight_ * cos_start_rotation;
+    const double weighted_sin_start_rotation =
+        translation_weight_ * sin_start_rotation;
+
+    // Jacobians in Ceres are ordered by the parameter blocks:
+    // jacobian[i] = [(dr_0 / dx_i)^T, ..., (dr_n / dx_i)^T].
+    if (jacobians[0] != NULL) {
+      jacobians[0][0] = weighted_cos_start_rotation;
+      jacobians[0][1] = weighted_sin_start_rotation;
+      jacobians[0][2] = weighted_sin_start_rotation * delta_x -
+                        weighted_cos_start_rotation * delta_y;
+      jacobians[0][3] = -weighted_sin_start_rotation;
+      jacobians[0][4] = weighted_cos_start_rotation;
+      jacobians[0][5] = weighted_cos_start_rotation * delta_x +
+                        weighted_sin_start_rotation * delta_y;
+      jacobians[0][6] = 0;
+      jacobians[0][7] = 0;
+      jacobians[0][8] = rotation_weight_;
+    }
+    if (jacobians[1] != NULL) {
+      jacobians[1][0] = -weighted_cos_start_rotation;
+      jacobians[1][1] = -weighted_sin_start_rotation;
+      jacobians[1][2] = 0;
+      jacobians[1][3] = weighted_sin_start_rotation;
+      jacobians[1][4] = -weighted_cos_start_rotation;
+      jacobians[1][5] = 0;
+      jacobians[1][6] = 0;
+      jacobians[1][7] = 0;
+      jacobians[1][8] = -rotation_weight_;
+    }
+    return true;
+  }
+
+ private:
+  const transform::Rigid2d observed_relative_pose_;
+  const double translation_weight_;
+  const double rotation_weight_;
+};
+
+}  // namespace
+
+ceres::CostFunction* CreateAutoDiffSpaCostFunction(
+    const PoseGraphInterface::Constraint::Pose& observed_relative_pose) {
+  return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,
+                                         3 /* start pose variables */,
+                                         3 /* end pose variables */>(
+      new SpaCostFunction2D(observed_relative_pose));
+}
+
+ceres::CostFunction* CreateAnalyticalSpaCostFunction(
+    const PoseGraphInterface::Constraint::Pose& observed_relative_pose) {
+  return new AnalyticalSpaCostFunction2D(observed_relative_pose);
+}
+
+}  // namespace optimization
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h

@@ -17,50 +17,18 @@
 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
 #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_
 
-#include <array>
+#include "cartographer/mapping/pose_graph_interface.h"
-
-#include "Eigen/Core"
-#include "Eigen/Geometry"
-#include "cartographer/common/math.h"
-#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
-#include "cartographer/mapping/pose_graph.h"
-#include "cartographer/transform/rigid_transform.h"
-#include "cartographer/transform/transform.h"
 #include "ceres/ceres.h"
-#include "ceres/jet.h"
 
 namespace cartographer {
 namespace mapping {
 namespace optimization {
 
-class SpaCostFunction2D {
+ceres::CostFunction* CreateAutoDiffSpaCostFunction(
- public:
+    const PoseGraphInterface::Constraint::Pose& pose);
-  static ceres::CostFunction* CreateAutoDiffCostFunction(
-      const PoseGraph::Constraint::Pose& pose) {
-    return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,
-                                           3 /* pose variables */,
-                                           3 /* pose variables */>(
-        new SpaCostFunction2D(pose));
-  }
 
-  template <typename T>
+ceres::CostFunction* CreateAnalyticalSpaCostFunction(
-  bool operator()(const T* const c_i, const T* const c_j, T* e) const {
+    const PoseGraphInterface::Constraint::Pose& pose);
-    using optimization::ComputeUnscaledError;
-    using optimization::ScaleError;
-
-    const std::array<T, 3> error = ScaleError(
-        ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j),
-        pose_.translation_weight, pose_.rotation_weight);
-    std::copy(std::begin(error), std::end(error), e);
-    return true;
-  }
-
- private:
-  explicit SpaCostFunction2D(const PoseGraph::Constraint::Pose& pose)
-      : pose_(pose) {}
-
-  const PoseGraph::Constraint::Pose pose_;
-};
 
 }  // namespace optimization
 }  // namespace mapping

cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc

@@ -0,0 +1,150 @@
+/*
+ * 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/optimization/cost_functions/spa_cost_function_2d.h"
+
+#include <memory>
+
+#include "cartographer/transform/rigid_transform.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+namespace cartographer {
+namespace mapping {
+namespace optimization {
+namespace {
+
+using ::testing::ElementsAre;
+
+constexpr int kPoseDimension = 3;
+constexpr int kResidualsCount = 3;
+constexpr int kParameterBlocksCount = 2;
+constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension;
+
+using ResidualType = std::array<double, kResidualsCount>;
+using JacobianType = std::array<std::array<double, kJacobianColDimension>,
+                                kParameterBlocksCount>;
+
+::testing::Matcher<double> Near(double expected, double precision = 1e-05) {
+  return testing::DoubleNear(expected, precision);
+}
+
+class SpaCostFunction2DTest : public ::testing::Test {
+ public:
+  SpaCostFunction2DTest()
+      : constraint_(PoseGraphInterface::Constraint::Pose{
+            transform::Rigid3d(Eigen::Vector3d(1., 1., 1.),
+                               Eigen::Quaterniond(1., 1., -1., -1.)),
+            1, 10}),
+        auto_diff_cost_(CreateAutoDiffSpaCostFunction(constraint_)),
+        analytical_cost_(CreateAnalyticalSpaCostFunction(constraint_)) {
+    for (int i = 0; i < kParameterBlocksCount; ++i) {
+      jacobian_ptrs_[i] = jacobian_[i].data();
+    }
+  }
+
+  std::pair<const ResidualType&, const JacobianType&> EvaluateAnalyticalCost(
+      const std::array<const double*, 2>& parameter_blocks) {
+    return Evaluate(parameter_blocks, analytical_cost_);
+  }
+
+  std::pair<const ResidualType&, const JacobianType&> EvaluateAutoDiffCost(
+      const std::array<const double*, 2>& parameter_blocks) {
+    return Evaluate(parameter_blocks, auto_diff_cost_);
+  }
+
+ private:
+  std::pair<const ResidualType&, const JacobianType&> Evaluate(
+      const std::array<const double*, 2>& parameter_blocks,
+      const std::unique_ptr<ceres::CostFunction>& cost_function) {
+    cost_function->Evaluate(parameter_blocks.data(), residuals_.data(),
+                            jacobian_ptrs_.data());
+    return std::make_pair(std::cref(residuals_), std::cref(jacobian_));
+  }
+
+  ResidualType residuals_;
+  JacobianType jacobian_;
+  std::array<double*, kParameterBlocksCount> jacobian_ptrs_;
+  PoseGraphInterface::Constraint::Pose constraint_;
+  std::unique_ptr<ceres::CostFunction> auto_diff_cost_;
+  std::unique_ptr<ceres::CostFunction> analytical_cost_;
+};
+
+TEST_F(SpaCostFunction2DTest, CompareAutoDiffAndAnalytical) {
+  std::array<double, 3> start_pose{{1., 1., 1.}};
+  std::array<double, 3> end_pose{{10., 1., 100.}};
+  std::array<const double*, 2> parameter_blocks{
+      {start_pose.data(), end_pose.data()}};
+
+  ResidualType auto_diff_residual, analytical_residual;
+  JacobianType auto_diff_jacobian, analytical_jacobian;
+  std::tie(auto_diff_residual, auto_diff_jacobian) =
+      EvaluateAutoDiffCost(parameter_blocks);
+  std::tie(analytical_residual, analytical_jacobian) =
+      EvaluateAnalyticalCost(parameter_blocks);
+
+  for (int i = 0; i < kResidualsCount; ++i) {
+    EXPECT_THAT(auto_diff_residual[i], Near(analytical_residual[i]));
+  }
+  for (int i = 0; i < kParameterBlocksCount; ++i) {
+    for (int j = 0; j < kJacobianColDimension; ++j) {
+      EXPECT_THAT(auto_diff_jacobian[i][j], Near(analytical_jacobian[i][j]));
+    }
+  }
+}
+
+TEST_F(SpaCostFunction2DTest, EvaluateAnalyticalCost) {
+  std::array<double, 3> start_pose{{1., 1., 1.}};
+  std::array<double, 3> end_pose{{10., 1., 100.}};
+  std::array<const double*, 2> parameter_blocks{
+      {start_pose.data(), end_pose.data()}};
+
+  auto residuals_and_jacobian = EvaluateAnalyticalCost(parameter_blocks);
+  EXPECT_THAT(residuals_and_jacobian.first,
+              ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333)));
+  EXPECT_THAT(
+      residuals_and_jacobian.second,
+      ElementsAre(
+          ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324),
+                      Near(-0.841471), Near(0.540302), Near(4.86272), Near(0),
+                      Near(0), Near(10)),
+          ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471),
+                      Near(-0.540302), Near(0), Near(0), Near(0), Near(-10))));
+}
+
+TEST_F(SpaCostFunction2DTest, EvaluateAutoDiffCost) {
+  std::array<double, 3> start_pose{{1., 1., 1.}};
+  std::array<double, 3> end_pose{{10., 1., 100.}};
+  std::array<const double*, 2> parameter_blocks{
+      {start_pose.data(), end_pose.data()}};
+
+  auto residuals_and_jacobian = EvaluateAutoDiffCost(parameter_blocks);
+  EXPECT_THAT(residuals_and_jacobian.first,
+              ElementsAre(Near(-3.86272), Near(8.57324), Near(-6.83333)));
+  EXPECT_THAT(
+      residuals_and_jacobian.second,
+      ElementsAre(
+          ElementsAre(Near(0.540302), Near(0.841471), Near(7.57324),
+                      Near(-0.841471), Near(0.540302), Near(4.86272), Near(0),
+                      Near(0), Near(10)),
+          ElementsAre(Near(-0.540302), Near(-0.841471), Near(0), Near(0.841471),
+                      Near(-0.540302), Near(0), Near(0), Near(0), Near(-10))));
+}
+
+}  // namespace
+}  // namespace optimization
+}  // namespace mapping
+}  // namespace cartographer

cartographer/mapping/internal/optimization/optimization_problem_2d.cc

@@ -237,7 +237,7 @@ void OptimizationProblem2D::Solve(
   // Add cost functions for intra- and inter-submap constraints.
   for (const Constraint& constraint : constraints) {
     problem.AddResidualBlock(
-        SpaCostFunction2D::CreateAutoDiffCostFunction(constraint.pose),
+        CreateAutoDiffSpaCostFunction(constraint.pose),
         // Only loop closure constraints should have a loss function.
         constraint.tag == Constraint::INTER_SUBMAP
             ? new ceres::HuberLoss(options_.huber_scale())
@@ -276,7 +276,7 @@ void OptimizationProblem2D::Solve(
                                         second_node_data);
       if (relative_odometry != nullptr) {
         problem.AddResidualBlock(
-            SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{
+            CreateAutoDiffSpaCostFunction(Constraint::Pose{
                 *relative_odometry, options_.odometry_translation_weight(),
                 options_.odometry_rotation_weight()}),
             nullptr /* loss function */, C_nodes.at(first_node_id).data(),
@@ -288,7 +288,7 @@ void OptimizationProblem2D::Solve(
           transform::Embed3D(first_node_data.local_pose_2d.inverse() *
                              second_node_data.local_pose_2d);
       problem.AddResidualBlock(
-          SpaCostFunction2D::CreateAutoDiffCostFunction(
+          CreateAutoDiffSpaCostFunction(
               Constraint::Pose{relative_local_slam_pose,
                                options_.local_slam_pose_translation_weight(),
                                options_.local_slam_pose_rotation_weight()}),