From 0ab591aa013edffb9b25ac4a7127c67e3d129621 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Wed, 18 Jul 2018 15:58:33 +0200 Subject: [PATCH] [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. --- .../cost_function/relative_pose_cost_2d.cc | 94 ++++++++++ .../cost_function/relative_pose_cost_2d.h | 51 ++++++ .../relative_pose_cost_2d_test.cc | 167 ++++++++++++++++++ .../pose_graph/proto/cost_function.proto | 11 +- 4 files changed, 319 insertions(+), 4 deletions(-) create mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc create mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h create mode 100644 cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc new file mode 100644 index 0000000..fae18cc --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.cc @@ -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 diff --git a/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h new file mode 100644 index 0000000..5395e89 --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d.h @@ -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_ 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 new file mode 100644 index 0000000..180c42e --- /dev/null +++ b/cartographer/pose_graph/constraint/cost_function/relative_pose_cost_2d_test.cc @@ -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; +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; + } + + 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(parameters); + analytical_cost_ = common::make_unique< + ceres::AutoDiffCostFunction>( + new AutoDiffRelativePoseCost(parameters)); + for (int i = 0; i < kParameterBlocksCount; ++i) { + jacobian_ptrs_[i] = jacobian_[i].data(); + } + } + + 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()); + return std::make_pair(std::cref(residuals_), std::cref(jacobian_)); + } + + ResidualType residuals_; + JacobianType jacobian_; + std::array jacobian_ptrs_; + std::unique_ptr auto_diff_cost_; + std::unique_ptr analytical_cost_; +}; + +::testing::Matcher Near(double expected) { + constexpr double kPrecision = 1e-05; + return ::testing::DoubleNear(expected, kPrecision); +} + +TEST_F(RelativePoseCost2DTest, CompareAutoDiffAndAnalytical) { + 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); + + 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 start_pose{{1., 1., 1.}}; + std::array end_pose{{10., 1., 100.}}; + std::array 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 diff --git a/cartographer/pose_graph/proto/cost_function.proto b/cartographer/pose_graph/proto/cost_function.proto index 048e7b7..ed43ddf 100644 --- a/cartographer/pose_graph/proto/cost_function.proto +++ b/cartographer/pose_graph/proto/cost_function.proto @@ -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; }