diff --git a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc new file mode 100644 index 0000000..4c3aee9 --- /dev/null +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.cc @@ -0,0 +1,150 @@ +/* + * 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/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h" + +#include + +#include "Eigen/Core" +#include "Eigen/Geometry" +#include "cartographer/common/math.h" +#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" +#include "cartographer/transform/rigid_transform.h" +#include "cartographer/transform/transform.h" +#include "ceres/jet.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +class SpaCostFunction2D { + public: + explicit SpaCostFunction2D( + const PoseGraphInterface::Constraint::Pose& observed_relative_pose) + : observed_relative_pose_(observed_relative_pose) {} + + template + bool operator()(const T* const start_pose, const T* const end_pose, + T* e) const { + const std::array error = + ScaleError(ComputeUnscaledError( + transform::Project2D(observed_relative_pose_.zbar_ij), + start_pose, end_pose), + observed_relative_pose_.translation_weight, + observed_relative_pose_.rotation_weight); + std::copy(std::begin(error), std::end(error), e); + return true; + } + + private: + const PoseGraphInterface::Constraint::Pose observed_relative_pose_; +}; + +class AnalyticalSpaCostFunction2D + : public ceres::SizedCostFunction<3 /* number of residuals */, + 3 /* size of start pose */, + 3 /* size of end pose */> { + public: + explicit AnalyticalSpaCostFunction2D( + const PoseGraphInterface::Constraint::Pose& constraint_pose) + : observed_relative_pose_(transform::Project2D(constraint_pose.zbar_ij)), + translation_weight_(constraint_pose.translation_weight), + rotation_weight_(constraint_pose.rotation_weight) {} + virtual ~AnalyticalSpaCostFunction2D() {} + + bool Evaluate(double const* const* parameters, double* residuals, + double** jacobians) const override { + 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_ * + (observed_relative_pose_.translation().x() - + (cos_start_rotation * delta_x + sin_start_rotation * delta_y)); + residuals[1] = + translation_weight_ * + (observed_relative_pose_.translation().y() - + (-sin_start_rotation * delta_x + cos_start_rotation * delta_y)); + residuals[2] = + rotation_weight_ * + common::NormalizeAngleDifference( + observed_relative_pose_.rotation().angle() - (end[2] - start[2])); + if (jacobians == NULL) 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] != NULL) { + 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] != NULL) { + 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; + } + + private: + const transform::Rigid2d observed_relative_pose_; + const double translation_weight_; + const double rotation_weight_; +}; + +} // namespace + +ceres::CostFunction* CreateAutoDiffSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& observed_relative_pose) { + return new ceres::AutoDiffCostFunction( + new SpaCostFunction2D(observed_relative_pose)); +} + +ceres::CostFunction* CreateAnalyticalSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& observed_relative_pose) { + return new AnalyticalSpaCostFunction2D(observed_relative_pose); +} + +} // namespace optimization +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h index 9020b47..01d9363 100644 --- a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h @@ -17,50 +17,18 @@ #ifndef CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ #define CARTOGRAPHER_MAPPING_INTERNAL_OPTIMIZATION_COST_FUNCTIONS_SPA_COST_FUNCTION_2D_H_ -#include - -#include "Eigen/Core" -#include "Eigen/Geometry" -#include "cartographer/common/math.h" -#include "cartographer/mapping/internal/optimization/cost_functions/cost_helpers.h" -#include "cartographer/mapping/pose_graph.h" -#include "cartographer/transform/rigid_transform.h" -#include "cartographer/transform/transform.h" +#include "cartographer/mapping/pose_graph_interface.h" #include "ceres/ceres.h" -#include "ceres/jet.h" namespace cartographer { namespace mapping { namespace optimization { -class SpaCostFunction2D { - public: - static ceres::CostFunction* CreateAutoDiffCostFunction( - const PoseGraph::Constraint::Pose& pose) { - return new ceres::AutoDiffCostFunction( - new SpaCostFunction2D(pose)); - } +ceres::CostFunction* CreateAutoDiffSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& pose); - template - bool operator()(const T* const c_i, const T* const c_j, T* e) const { - using optimization::ComputeUnscaledError; - using optimization::ScaleError; - - const std::array error = ScaleError( - ComputeUnscaledError(transform::Project2D(pose_.zbar_ij), c_i, c_j), - pose_.translation_weight, pose_.rotation_weight); - std::copy(std::begin(error), std::end(error), e); - return true; - } - - private: - explicit SpaCostFunction2D(const PoseGraph::Constraint::Pose& pose) - : pose_(pose) {} - - const PoseGraph::Constraint::Pose pose_; -}; +ceres::CostFunction* CreateAnalyticalSpaCostFunction( + const PoseGraphInterface::Constraint::Pose& pose); } // namespace optimization } // namespace mapping diff --git a/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc new file mode 100644 index 0000000..409ba5d --- /dev/null +++ b/cartographer/mapping/internal/optimization/cost_functions/spa_cost_function_2d_test.cc @@ -0,0 +1,150 @@ +/* + * 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/mapping/internal/optimization/cost_functions/spa_cost_function_2d.h" + +#include + +#include "cartographer/transform/rigid_transform.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace cartographer { +namespace mapping { +namespace optimization { +namespace { + +using ::testing::ElementsAre; + +constexpr int kPoseDimension = 3; +constexpr int kResidualsCount = 3; +constexpr int kParameterBlocksCount = 2; +constexpr int kJacobianColDimension = kResidualsCount * kPoseDimension; + +using ResidualType = std::array; +using JacobianType = std::array, + kParameterBlocksCount>; + +::testing::Matcher Near(double expected, double precision = 1e-05) { + return testing::DoubleNear(expected, precision); +} + +class SpaCostFunction2DTest : public ::testing::Test { + public: + SpaCostFunction2DTest() + : constraint_(PoseGraphInterface::Constraint::Pose{ + transform::Rigid3d(Eigen::Vector3d(1., 1., 1.), + Eigen::Quaterniond(1., 1., -1., -1.)), + 1, 10}), + auto_diff_cost_(CreateAutoDiffSpaCostFunction(constraint_)), + analytical_cost_(CreateAnalyticalSpaCostFunction(constraint_)) { + for (int i = 0; i < kParameterBlocksCount; ++i) { + jacobian_ptrs_[i] = jacobian_[i].data(); + } + } + + std::pair EvaluateAnalyticalCost( + 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_; + PoseGraphInterface::Constraint::Pose constraint_; + std::unique_ptr auto_diff_cost_; + std::unique_ptr analytical_cost_; +}; + +TEST_F(SpaCostFunction2DTest, 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) = + EvaluateAnalyticalCost(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(SpaCostFunction2DTest, EvaluateAnalyticalCost) { + 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 = EvaluateAnalyticalCost(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)))); +} + +TEST_F(SpaCostFunction2DTest, EvaluateAutoDiffCost) { + 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 = EvaluateAutoDiffCost(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 optimization +} // namespace mapping +} // namespace cartographer diff --git a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc index 8a91277..76da8b0 100644 --- a/cartographer/mapping/internal/optimization/optimization_problem_2d.cc +++ b/cartographer/mapping/internal/optimization/optimization_problem_2d.cc @@ -237,7 +237,7 @@ void OptimizationProblem2D::Solve( // Add cost functions for intra- and inter-submap constraints. for (const Constraint& constraint : constraints) { problem.AddResidualBlock( - SpaCostFunction2D::CreateAutoDiffCostFunction(constraint.pose), + CreateAutoDiffSpaCostFunction(constraint.pose), // Only loop closure constraints should have a loss function. constraint.tag == Constraint::INTER_SUBMAP ? new ceres::HuberLoss(options_.huber_scale()) @@ -276,7 +276,7 @@ void OptimizationProblem2D::Solve( second_node_data); if (relative_odometry != nullptr) { problem.AddResidualBlock( - SpaCostFunction2D::CreateAutoDiffCostFunction(Constraint::Pose{ + CreateAutoDiffSpaCostFunction(Constraint::Pose{ *relative_odometry, options_.odometry_translation_weight(), options_.odometry_rotation_weight()}), nullptr /* loss function */, C_nodes.at(first_node_id).data(), @@ -288,7 +288,7 @@ void OptimizationProblem2D::Solve( transform::Embed3D(first_node_data.local_pose_2d.inverse() * second_node_data.local_pose_2d); problem.AddResidualBlock( - SpaCostFunction2D::CreateAutoDiffCostFunction( + CreateAutoDiffSpaCostFunction( Constraint::Pose{relative_local_slam_pose, options_.local_slam_pose_translation_weight(), options_.local_slam_pose_rotation_weight()}),