From 1b455e57e57c90e3903ddddd37a52d007aac6307 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Fri, 20 Jul 2018 10:55:05 +0200 Subject: [PATCH] [GenericPoseGraph] Use GradientChecker to check gradients (Ba-Dum-Tss). (#1306) Ba-Dum-Tss! --- .../relative_pose_cost_2d_test.cc | 104 +++++------------- 1 file changed, 29 insertions(+), 75 deletions(-) diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc index 9a1cb10..4ca480d 100644 --- a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc +++ b/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,53 +38,20 @@ using ResidualType = std::array; using JacobianType = std::array, 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 - bool operator()(const T* const start_pose, const T* const end_pose, - T* e) const { - const std::array 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; +constexpr char kParameters[] = R"PROTO( + first_t_second { + translation: { x: 1 y: 1 } + rotation: -2.214297 } - - private: - const double translation_weight_; - const double rotation_weight_; - const transform::Rigid2d first_T_second_; -}; + translation_weight: 1 + rotation_weight: 10 +)PROTO"; class RelativePoseCost2DTest : public ::testing::Test { public: - RelativePoseCost2DTest() { - constexpr char kParameters[] = R"PROTO( - first_t_second { - translation: { x: 1 y: 1 } - rotation: -2.214297 - } - translation_weight: 1 - rotation_weight: 10 - )PROTO"; - - auto parameters = - ParseProto(kParameters); - auto_diff_cost_ = common::make_unique(parameters); - analytical_cost_ = common::make_unique< - ceres::AutoDiffCostFunction>( - new AutoDiffRelativePoseCost(parameters)); + RelativePoseCost2DTest() + : relative_pose_cost_2d_(common::make_unique( + ParseProto(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 EvaluateRelativePoseCost2D( const std::array& parameter_blocks) { - return Evaluate(parameter_blocks, analytical_cost_); - } - - std::pair EvaluateAutoDiffCost( - const std::array& parameter_blocks) { - return Evaluate(parameter_blocks, auto_diff_cost_); - } - - private: - std::pair Evaluate( - const std::array& parameter_blocks, - const std::unique_ptr& cost_function) { - cost_function->Evaluate(parameter_blocks.data(), residuals_.data(), - jacobian_ptrs_.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 jacobian_ptrs_; - std::unique_ptr auto_diff_cost_; - std::unique_ptr analytical_cost_; + std::unique_ptr 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 start_pose{{1., 1., 1.}}; std::array end_pose{{10., 1., 100.}}; std::array 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) {