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

Ba-Dum-Tss!
master
Alexander Belyaev 2018-07-20 10:55:05 +02:00 committed by Wally B. Feed
parent c473a65a5e
commit 1b455e57e5
1 changed files with 29 additions and 75 deletions

View File

@ -17,8 +17,8 @@
#include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h" #include "cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h"
#include "cartographer/common/make_unique.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 "cartographer/pose_graph/internal/testing/test_helpers.h"
#include "ceres/gradient_checker.h"
namespace cartographer { namespace cartographer {
namespace pose_graph { namespace pose_graph {
@ -30,6 +30,7 @@ constexpr int kParameterBlocksCount = 2;
constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension; constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension;
using ::testing::ElementsAre; using ::testing::ElementsAre;
using testing::EqualsProto;
using testing::Near; using testing::Near;
using testing::ParseProto; using testing::ParseProto;
@ -37,37 +38,6 @@ using ResidualType = std::array<double, kResidualsCount>;
using JacobianType = std::array<std::array<double, kJacobianColDimension>, using JacobianType = std::array<std::array<double, kJacobianColDimension>,
kParameterBlocksCount>; 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( constexpr char kParameters[] = R"PROTO(
first_t_second { first_t_second {
translation: { x: 1 y: 1 } translation: { x: 1 y: 1 }
@ -77,13 +47,11 @@ class RelativePoseCost2DTest : public ::testing::Test {
rotation_weight: 10 rotation_weight: 10
)PROTO"; )PROTO";
auto parameters = class RelativePoseCost2DTest : public ::testing::Test {
ParseProto<proto::RelativePose2D::Parameters>(kParameters); public:
auto_diff_cost_ = common::make_unique<RelativePoseCost2D>(parameters); RelativePoseCost2DTest()
analytical_cost_ = common::make_unique< : relative_pose_cost_2d_(common::make_unique<RelativePoseCost2D>(
ceres::AutoDiffCostFunction<AutoDiffRelativePoseCost, kResidualsCount, ParseProto<proto::RelativePose2D::Parameters>(kParameters))) {
kPoseDimension, kPoseDimension>>(
new AutoDiffRelativePoseCost(parameters));
for (int i = 0; i < kParameterBlocksCount; ++i) { for (int i = 0; i < kParameterBlocksCount; ++i) {
jacobian_ptrs_[i] = jacobian_[i].data(); jacobian_ptrs_[i] = jacobian_[i].data();
} }
@ -92,51 +60,37 @@ class RelativePoseCost2DTest : public ::testing::Test {
std::pair<const ResidualType&, const JacobianType&> std::pair<const ResidualType&, const JacobianType&>
EvaluateRelativePoseCost2D( EvaluateRelativePoseCost2D(
const std::array<const double*, 2>& parameter_blocks) { const std::array<const double*, 2>& parameter_blocks) {
return Evaluate(parameter_blocks, analytical_cost_); relative_pose_cost_2d_->Evaluate(parameter_blocks.data(), residuals_.data(),
}
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()); jacobian_ptrs_.data());
return std::make_pair(std::cref(residuals_), std::cref(jacobian_)); return std::make_pair(std::cref(residuals_), std::cref(jacobian_));
} }
protected:
ResidualType residuals_; ResidualType residuals_;
JacobianType jacobian_; JacobianType jacobian_;
std::array<double*, kParameterBlocksCount> jacobian_ptrs_; std::array<double*, kParameterBlocksCount> jacobian_ptrs_;
std::unique_ptr<ceres::CostFunction> auto_diff_cost_; std::unique_ptr<RelativePoseCost2D> relative_pose_cost_2d_;
std::unique_ptr<ceres::CostFunction> analytical_cost_;
}; };
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> start_pose{{1., 1., 1.}};
std::array<double, kPoseDimension> end_pose{{10., 1., 100.}}; std::array<double, kPoseDimension> end_pose{{10., 1., 100.}};
std::array<const double*, kParameterBlocksCount> parameter_blocks{ std::array<const double*, kParameterBlocksCount> parameter_blocks{
{start_pose.data(), end_pose.data()}}; {start_pose.data(), end_pose.data()}};
ResidualType auto_diff_residual, analytical_residual; using ::ceres::GradientChecker;
JacobianType auto_diff_jacobian, analytical_jacobian; GradientChecker gradient_checker(relative_pose_cost_2d_.get(),
std::tie(auto_diff_residual, auto_diff_jacobian) = {} /* local parameterizations */,
EvaluateAutoDiffCost(parameter_blocks); ceres::NumericDiffOptions{});
std::tie(analytical_residual, analytical_jacobian) =
EvaluateRelativePoseCost2D(parameter_blocks);
for (int i = 0; i < kResidualsCount; ++i) { GradientChecker::ProbeResults probe_results;
EXPECT_THAT(auto_diff_residual[i], Near(analytical_residual[i])); gradient_checker.Probe(parameter_blocks.data(),
} 1e-08 /* relative precision */, &probe_results);
for (int i = 0; i < kParameterBlocksCount; ++i) { EXPECT_TRUE(probe_results.return_value);
for (int j = 0; j < kJacobianColDimension; ++j) {
EXPECT_THAT(auto_diff_jacobian[i][j], Near(analytical_jacobian[i][j]));
}
}
} }
TEST_F(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) { TEST_F(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) {