[GenericPoseGraph] Add RelativePoseCost2D. (#1295)
This is the implementation of the ceres::CostFunction that uses the new proto definition. It is tested using the Autodiff version.master
Alexander Belyaev
Wally B. Feed
parent
ce9c567e3c
commit
0ab591aa01
|
|
@ -0,0 +1,94 @@
|
|||
/*
|
||||
* 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/pose_graph/constraint/cost_function/relative_pose_cost_2d.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
RelativePoseCost2D::RelativePoseCost2D(
|
||||
const proto::RelativePose2D::Parameters& parameters)
|
||||
: translation_weight_(parameters.translation_weight()),
|
||||
rotation_weight_(parameters.rotation_weight()),
|
||||
first_T_second_(transform::ToRigid2(parameters.first_t_second())) {}
|
||||
|
||||
proto::RelativePose2D::Parameters RelativePoseCost2D::ToProto() const {
|
||||
proto::RelativePose2D::Parameters parameters;
|
||||
parameters.set_translation_weight(translation_weight_);
|
||||
parameters.set_rotation_weight(rotation_weight_);
|
||||
*parameters.mutable_first_t_second() = transform::ToProto(first_T_second_);
|
||||
return parameters;
|
||||
}
|
||||
|
||||
bool RelativePoseCost2D::Evaluate(double const* const* parameters,
|
||||
double* residuals, double** jacobians) const {
|
||||
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_ *
|
||||
(first_T_second_.translation().x() -
|
||||
(cos_start_rotation * delta_x + sin_start_rotation * delta_y));
|
||||
residuals[1] =
|
||||
translation_weight_ *
|
||||
(first_T_second_.translation().y() -
|
||||
(-sin_start_rotation * delta_x + cos_start_rotation * delta_y));
|
||||
residuals[2] = rotation_weight_ *
|
||||
common::NormalizeAngleDifference(
|
||||
first_T_second_.rotation().angle() - (end[2] - start[2]));
|
||||
if (jacobians == nullptr) 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] != nullptr) {
|
||||
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] != nullptr) {
|
||||
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;
|
||||
}
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
/*
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_
|
||||
#define CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_
|
||||
|
||||
#include "cartographer/pose_graph/proto/cost_function.pb.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "ceres/sized_cost_function.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
|
||||
class RelativePoseCost2D
|
||||
: public ceres::SizedCostFunction<3 /* number of residuals */,
|
||||
3 /* size of first pose */,
|
||||
3 /* size of second pose */> {
|
||||
public:
|
||||
explicit RelativePoseCost2D(
|
||||
const proto::RelativePose2D::Parameters& parameters);
|
||||
|
||||
proto::RelativePose2D::Parameters ToProto() const;
|
||||
|
||||
// Parameters are packed as [first_pose_2d, second_pose_2d], where each 2D
|
||||
// pose is [translation_x, translation_y, rotation].
|
||||
bool Evaluate(double const* const* parameters, double* residuals,
|
||||
double** jacobians) const final;
|
||||
|
||||
private:
|
||||
const double translation_weight_;
|
||||
const double rotation_weight_;
|
||||
const transform::Rigid2d first_T_second_;
|
||||
};
|
||||
|
||||
} // namespace pose_graph
|
||||
} // namespace cartographer
|
||||
|
||||
#endif // CARTOGRAPHER_POSE_GRAPH_CONSTRAINT_COST_FUNCTION_RELATIVE_POSE_COST_2D_H_
|
||||
|
|
@ -0,0 +1,167 @@
|
|||
/*
|
||||
* 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/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 "gmock/gmock.h"
|
||||
#include "google/protobuf/text_format.h"
|
||||
|
||||
namespace cartographer {
|
||||
namespace pose_graph {
|
||||
namespace {
|
||||
|
||||
constexpr int kPoseDimension = 3;
|
||||
constexpr int kResidualsCount = 3;
|
||||
constexpr int kParameterBlocksCount = 2;
|
||||
constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension;
|
||||
|
||||
using ::google::protobuf::TextFormat;
|
||||
using ::testing::ElementsAre;
|
||||
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() {
|
||||
proto::RelativePose2D::Parameters parameters;
|
||||
constexpr char kParameters[] = R"PROTO(
|
||||
first_t_second {
|
||||
translation: { x: 1 y: 1 }
|
||||
rotation: -2.214297
|
||||
}
|
||||
translation_weight: 1
|
||||
rotation_weight: 10
|
||||
)PROTO";
|
||||
EXPECT_TRUE(TextFormat::ParseFromString(kParameters, ¶meters));
|
||||
|
||||
auto_diff_cost_ = common::make_unique<RelativePoseCost2D>(parameters);
|
||||
analytical_cost_ = common::make_unique<
|
||||
ceres::AutoDiffCostFunction<AutoDiffRelativePoseCost, kResidualsCount,
|
||||
kPoseDimension, kPoseDimension>>(
|
||||
new AutoDiffRelativePoseCost(parameters));
|
||||
for (int i = 0; i < kParameterBlocksCount; ++i) {
|
||||
jacobian_ptrs_[i] = jacobian_[i].data();
|
||||
}
|
||||
}
|
||||
|
||||
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(),
|
||||
jacobian_ptrs_.data());
|
||||
return std::make_pair(std::cref(residuals_), std::cref(jacobian_));
|
||||
}
|
||||
|
||||
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_;
|
||||
};
|
||||
|
||||
::testing::Matcher<double> Near(double expected) {
|
||||
constexpr double kPrecision = 1e-05;
|
||||
return ::testing::DoubleNear(expected, kPrecision);
|
||||
}
|
||||
|
||||
TEST_F(RelativePoseCost2DTest, CompareAutoDiffAndAnalytical) {
|
||||
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);
|
||||
|
||||
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(RelativePoseCost2DTest, EvaluateRelativePoseCost2D) {
|
||||
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()}};
|
||||
|
||||
auto residuals_and_jacobian = EvaluateRelativePoseCost2D(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 pose_graph
|
||||
} // namespace cartographer
|
||||
|
|
@ -23,16 +23,19 @@ message RelativePose2D {
|
|||
NodeId first = 1;
|
||||
NodeId second = 2;
|
||||
|
||||
transform.proto.Rigid2d first_T_second = 3;
|
||||
double translation_weight = 4;
|
||||
double rotation_weight = 5;
|
||||
message Parameters {
|
||||
transform.proto.Rigid2d first_t_second = 1;
|
||||
double translation_weight = 2;
|
||||
double rotation_weight = 3;
|
||||
}
|
||||
Parameters parameters = 3;
|
||||
}
|
||||
|
||||
message RelativePose3D {
|
||||
NodeId first = 1;
|
||||
NodeId second = 2;
|
||||
|
||||
transform.proto.Rigid3d first_T_second = 3;
|
||||
transform.proto.Rigid3d first_t_second = 3;
|
||||
double translation_weight = 4;
|
||||
double rotation_weight = 5;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue