Add analytical 2d cost function for pose graph (not used yet). (#1161)
Alexander Belyaev
GitHub
parent
f4c4d2ad40
commit
2f9ee597d0
|
|
@ -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 <array>
|
||||
|
||||
#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 <typename T>
|
||||
bool operator()(const T* const start_pose, const T* const end_pose,
|
||||
T* e) const {
|
||||
const std::array<T, 3> 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<SpaCostFunction2D, 3 /* residuals */,
|
||||
3 /* start pose variables */,
|
||||
3 /* end pose variables */>(
|
||||
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
|
||||
|
|
@ -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 <array>
|
||||
|
||||
#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<SpaCostFunction2D, 3 /* residuals */,
|
||||
3 /* pose variables */,
|
||||
3 /* pose variables */>(
|
||||
new SpaCostFunction2D(pose));
|
||||
}
|
||||
ceres::CostFunction* CreateAutoDiffSpaCostFunction(
|
||||
const PoseGraphInterface::Constraint::Pose& pose);
|
||||
|
||||
template <typename T>
|
||||
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
|
||||
using optimization::ComputeUnscaledError;
|
||||
using optimization::ScaleError;
|
||||
|
||||
const std::array<T, 3> 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
|
||||
|
|
|
|||
|
|
@ -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 <memory>
|
||||
|
||||
#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<double, kResidualsCount>;
|
||||
using JacobianType = std::array<std::array<double, kJacobianColDimension>,
|
||||
kParameterBlocksCount>;
|
||||
|
||||
::testing::Matcher<double> 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<const ResidualType&, const JacobianType&> EvaluateAnalyticalCost(
|
||||
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_;
|
||||
PoseGraphInterface::Constraint::Pose constraint_;
|
||||
std::unique_ptr<ceres::CostFunction> auto_diff_cost_;
|
||||
std::unique_ptr<ceres::CostFunction> analytical_cost_;
|
||||
};
|
||||
|
||||
TEST_F(SpaCostFunction2DTest, CompareAutoDiffAndAnalytical) {
|
||||
std::array<double, 3> start_pose{{1., 1., 1.}};
|
||||
std::array<double, 3> end_pose{{10., 1., 100.}};
|
||||
std::array<const double*, 2> 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<double, 3> start_pose{{1., 1., 1.}};
|
||||
std::array<double, 3> end_pose{{10., 1., 100.}};
|
||||
std::array<const double*, 2> 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<double, 3> start_pose{{1., 1., 1.}};
|
||||
std::array<double, 3> end_pose{{10., 1., 100.}};
|
||||
std::array<const double*, 2> 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
|
||||
|
|
@ -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()}),
|
||||
|
|
|
|||
Loading…
Reference in New Issue