[GenericPoseGraph] Use GradientChecker to check gradients (Ba-Dum-Tss). (#1306)

Ba-Dum-Tss!

cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc

@@ -17,8 +17,8 @@
 #include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h"
 
 #include "cartographer/common/make_unique.h"
-#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h"
 #include "cartographer/pose_graph/internal/testing/test_helpers.h"
+#include "ceres/gradient_checker.h"
 
 namespace cartographer {
 namespace pose_graph {
@@ -30,6 +30,7 @@ constexpr int kParameterBlocksCount = 2;
 constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension;
 
 using ::testing::ElementsAre;
+using testing::EqualsProto;
 using testing::Near;
 using testing::ParseProto;
 
@@ -37,37 +38,6 @@ using ResidualType = std::array<double, kResidualsCount>;
 using JacobianType = std::array<std::array<double, kJacobianColDimension>,
                                 kParameterBlocksCount>;
 
-// This is the autodiff version of the RelativePoseCost2D.
-//
-// TODO(pifon): Use the gradient_checker from Ceres.
-class AutoDiffRelativePoseCost {
- public:
-  explicit AutoDiffRelativePoseCost(
-      const proto::RelativePose2D::Parameters& parameters)
-      : translation_weight_(parameters.translation_weight()),
-        rotation_weight_(parameters.rotation_weight()),
-        first_T_second_(transform::ToRigid2(parameters.first_t_second())) {}
-
-  template <typename T>
-  bool operator()(const T* const start_pose, const T* const end_pose,
-                  T* e) const {
-    const std::array<T, 3> error = mapping::optimization::ScaleError(
-        mapping::optimization::ComputeUnscaledError(first_T_second_, start_pose,
-                                                    end_pose),
-        translation_weight_, rotation_weight_);
-    std::copy(std::begin(error), std::end(error), e);
-    return true;
-  }
-
- private:
-  const double translation_weight_;
-  const double rotation_weight_;
-  const transform::Rigid2d first_T_second_;
-};
-
-class RelativePoseCost2DTest : public ::testing::Test {
- public:
-  RelativePoseCost2DTest() {
 constexpr char kParameters[] = R"PROTO(
   first_t_second {
     translation: { x: 1 y: 1 }
@@ -77,13 +47,11 @@ class RelativePoseCost2DTest : public ::testing::Test {
   rotation_weight: 10
 )PROTO";
 
-    auto parameters =
-        ParseProto<proto::RelativePose2D::Parameters>(kParameters);
-    auto_diff_cost_ = common::make_unique<RelativePoseCost2D>(parameters);
-    analytical_cost_ = common::make_unique<
-        ceres::AutoDiffCostFunction<AutoDiffRelativePoseCost, kResidualsCount,
-                                    kPoseDimension, kPoseDimension>>(
-        new AutoDiffRelativePoseCost(parameters));
+class RelativePoseCost2DTest : public ::testing::Test {
+ public:
+  RelativePoseCost2DTest()
+      : relative_pose_cost_2d_(common::make_unique<RelativePoseCost2D>(
+            ParseProto<proto::RelativePose2D::Parameters>(kParameters))) {
     for (int i = 0; i < kParameterBlocksCount; ++i) {
       jacobian_ptrs_[i] = jacobian_[i].data();
     }
@@ -92,51 +60,37 @@ class RelativePoseCost2DTest : public ::testing::Test {
   std::pair<const ResidualType&, const JacobianType&>
   EvaluateRelativePoseCost2D(
       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(),
+    relative_pose_cost_2d_->Evaluate(parameter_blocks.data(), residuals_.data(),
                                      jacobian_ptrs_.data());
     return std::make_pair(std::cref(residuals_), std::cref(jacobian_));
   }
 
+ protected:
   ResidualType residuals_;
   JacobianType jacobian_;
   std::array<double*, kParameterBlocksCount> jacobian_ptrs_;
-  std::unique_ptr<ceres::CostFunction> auto_diff_cost_;
-  std::unique_ptr<ceres::CostFunction> analytical_cost_;
+  std::unique_ptr<RelativePoseCost2D> relative_pose_cost_2d_;
 };
 
-TEST_F(RelativePoseCost2DTest, CompareAutoDiffAndAnalytical) {
+TEST_F(RelativePoseCost2DTest, SerializesCorrectly) {
+  EXPECT_THAT(relative_pose_cost_2d_->ToProto(), EqualsProto(kParameters));
+}
+
+TEST_F(RelativePoseCost2DTest, CheckGradient) {
   std::array<double, kPoseDimension> start_pose{{1., 1., 1.}};
   std::array<double, kPoseDimension> end_pose{{10., 1., 100.}};
   std::array<const double*, kParameterBlocksCount> 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) =
-      EvaluateRelativePoseCost2D(parameter_blocks);
+  using ::ceres::GradientChecker;
+  GradientChecker gradient_checker(relative_pose_cost_2d_.get(),
+                                   {} /* local parameterizations */,
+                                   ceres::NumericDiffOptions{});
 
-  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]));
-    }
-  }
+  GradientChecker::ProbeResults probe_results;
+  gradient_checker.Probe(parameter_blocks.data(),
+                         1e-08 /* relative precision */, &probe_results);
+  EXPECT_TRUE(probe_results.return_value);
 }
 
 TEST_F(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) {