Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/3d/scan_matching/rotation_delta_cost_functor_3d.h"
00018
00019 #include <array>
00020
00021 #include "gtest/gtest.h"
00022
00023 namespace cartographer {
00024 namespace mapping {
00025 namespace scan_matching {
00026 namespace {
00027
00028 constexpr double kPrecision = 1e-8;
00029
00030 double ComputeRotationDeltaSquaredCost(
00031 const Eigen::Quaterniond& rotation, const double scaling_factor,
00032 const Eigen::Quaterniond& target_rotation) {
00033 std::unique_ptr<ceres::CostFunction> cost_function(
00034 RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(scaling_factor,
00035 target_rotation));
00036 const std::array<double, 4> parameter_quaternion = {
00037 {rotation.w(), rotation.x(), rotation.y(), rotation.z()}};
00038 const std::vector<const double*> parameters = {parameter_quaternion.data()};
00039 std::vector<double> residuals(cost_function->num_residuals());
00040 EXPECT_TRUE(cost_function->Evaluate(parameters.data(), residuals.data(),
00041 nullptr ));
00042 double sum_of_squares = 0;
00043 for (double residual : residuals) {
00044 sum_of_squares += residual * residual;
00045 }
00046 return sum_of_squares;
00047 }
00048
00049 TEST(RotationDeltaCostFunctor3DTest, SameRotationGivesZeroCost) {
00050 EXPECT_NEAR(
00051 0.,
00052 ComputeRotationDeltaSquaredCost(Eigen::Quaterniond::Identity(), 1.0,
00053 Eigen::Quaterniond::Identity()),
00054 kPrecision);
00055
00056 Eigen::Quaterniond rotation(
00057 Eigen::AngleAxisd(0.9, Eigen::Vector3d(0.2, 0.1, 0.3).normalized()));
00058 EXPECT_NEAR(0., ComputeRotationDeltaSquaredCost(rotation, 1.0, rotation),
00059 kPrecision);
00060 }
00061
00062 TEST(RotationDeltaCostFunctor3DTest, ComputesCorrectCost) {
00063 double scaling_factor = 1.2;
00064 double angle = 0.8;
00065 Eigen::Quaterniond rotation(
00066 Eigen::AngleAxisd(angle, Eigen::Vector3d(0.2, 0.1, 0.8).normalized()));
00067 Eigen::Quaterniond target_rotation(
00068 Eigen::AngleAxisd(0.2, Eigen::Vector3d(-0.5, 0.3, 0.4).normalized()));
00069 double expected_cost = std::pow(scaling_factor * std::sin(angle / 2.0), 2);
00070 EXPECT_NEAR(expected_cost,
00071 ComputeRotationDeltaSquaredCost(rotation, scaling_factor,
00072 Eigen::Quaterniond::Identity()),
00073 kPrecision);
00074 EXPECT_NEAR(expected_cost,
00075 ComputeRotationDeltaSquaredCost(target_rotation * rotation,
00076 scaling_factor, target_rotation),
00077 kPrecision);
00078 EXPECT_NEAR(expected_cost,
00079 ComputeRotationDeltaSquaredCost(rotation * target_rotation,
00080 scaling_factor, target_rotation),
00081 kPrecision);
00082 }
00083
00084 }
00085 }
00086 }
00087 }