12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc
View File

@ -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<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(
constexpr char kParameters[] = R"PROTO(
first_t_second {
translation: { x: 1 y: 1 }
rotation: -2.214297
}
translation_weight: 1
rotation_weight: 10
)PROTO";
)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) {