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_3d.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(LandmarkCostFunction3DTest, SmokeTest) {
00037 NodeSpec3D prev_node;
00038 prev_node.time = common::FromUniversal(0);
00039 NodeSpec3D next_node;
00040 next_node.time = common::FromUniversal(10);
00041
00042 std::unique_ptr<ceres::CostFunction> cost_function(
00043 LandmarkCostFunction3D::CreateAutoDiffCostFunction(
00044 LandmarkObservation{
00045 0 ,
00046 common::FromUniversal(5) ,
00047 transform::Rigid3d::Translation(Eigen::Vector3d(1., 1., 1.)),
00048 1. ,
00049 2. ,
00050 },
00051 prev_node, next_node));
00052
00053 const std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
00054 const std::array<double, 3> prev_node_translation{{0., 0., 0.}};
00055 const std::array<double, 4> next_node_rotation{{1., 0., 0., 0.}};
00056 const std::array<double, 3> next_node_translation{{2., 2., 2.}};
00057 const std::array<double, 4> landmark_rotation{{1., 0., 0., 0.}};
00058 const std::array<double, 3> landmark_translation{{1., 2., 2.}};
00059 const std::array<const double*, 6> parameter_blocks{
00060 {prev_node_rotation.data(), prev_node_translation.data(),
00061 next_node_rotation.data(), next_node_translation.data(),
00062 landmark_rotation.data(), landmark_translation.data()}};
00063
00064 std::array<double, 6> residuals;
00065 std::array<std::array<double, 21>, 6> jacobians;
00066 std::array<double*, 6> jacobians_ptrs;
00067 for (int i = 0; i < 6; ++i) jacobians_ptrs[i] = jacobians[i].data();
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 }