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/optimization/cost_functions/landmark_cost_function_2d.h"
00018
00019 #include <memory>
00020
00021 #include "cartographer/transform/rigid_transform.h"
00022 #include "gmock/gmock.h"
00023 #include "gtest/gtest.h"
00024
00025 namespace cartographer {
00026 namespace mapping {
00027 namespace optimization {
00028 namespace {
00029
00030 using ::testing::DoubleEq;
00031 using ::testing::ElementsAre;
00032
00033 using LandmarkObservation =
00034 PoseGraphInterface::LandmarkNode::LandmarkObservation;
00035
00036 TEST(LandmarkCostFunctionTest, SmokeTest) {
00037 NodeSpec2D prev_node;
00038 prev_node.time = common::FromUniversal(0);
00039 prev_node.gravity_alignment = Eigen::Quaterniond::Identity();
00040 NodeSpec2D next_node;
00041 next_node.time = common::FromUniversal(10);
00042 next_node.gravity_alignment = Eigen::Quaterniond::Identity();
00043
00044 std::unique_ptr<ceres::CostFunction> cost_function(
00045 LandmarkCostFunction2D::CreateAutoDiffCostFunction(
00046 LandmarkObservation{
00047 0 ,
00048 common::FromUniversal(5) ,
00049 transform::Rigid3d::Translation(Eigen::Vector3d(1., 1., 1.)),
00050 1. ,
00051 2. ,
00052 },
00053 prev_node, next_node));
00054
00055 const std::array<double, 3> prev_node_pose{{2., 0., 0.}};
00056 const std::array<double, 3> next_node_pose{{0., 2., 0.}};
00057 const std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
00058 const std::array<double, 3> landmark_translation{{1., 2., 1.}};
00059 const std::array<const double*, 4> parameter_blocks{
00060 {prev_node_pose.data(), next_node_pose.data(), landmark_rotation.data(),
00061 landmark_translation.data()}};
00062
00063 std::array<double, 6> residuals;
00064 std::array<std::array<double, 13>, 6> jacobians;
00065 std::array<double*, 6> jacobians_ptrs;
00066 for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data();
00067
00068 cost_function->Evaluate(parameter_blocks.data(), residuals.data(),
00069 jacobians_ptrs.data());
00070 EXPECT_THAT(residuals, ElementsAre(DoubleEq(1.), DoubleEq(0.), DoubleEq(0.),
00071 DoubleEq(0.), DoubleEq(0.), DoubleEq(0.)));
00072 }
00073
00074 }
00075 }
00076 }
00077 }