27 #include "gtest/gtest.h" 30 namespace mapping_3d {
31 namespace scan_matching {
34 class CeresScanMatcherTest :
public ::testing::Test {
36 CeresScanMatcherTest()
39 transform::
Rigid3d::Translation(
Eigen::Vector3d(-1., 0., 0.))) {
40 for (
const auto& point :
41 {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
42 Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
43 Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
44 Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
52 occupied_space_weight_0 = 1., 53 translation_weight = 0.01, 54 rotation_weight = 0.1, 55 only_optimize_yaw = false, 56 ceres_solver_options = { 57 use_nonmonotonic_steps = true, 58 max_num_iterations = 10, 69 ceres::Solver::Summary summary;
73 EXPECT_NEAR(0., summary.final_cost, 1e-2) << summary.FullReport();
84 TEST_F(CeresScanMatcherTest, PerfectEstimate) {
89 TEST_F(CeresScanMatcherTest, AlongX) {
95 TEST_F(CeresScanMatcherTest, AlongZ) {
100 TEST_F(CeresScanMatcherTest, AlongXYZ) {
105 TEST_F(CeresScanMatcherTest, FullPoseCorrection) {
108 Eigen::AngleAxisd(0.05, Eigen::Vector3d(0., 0., 1.)));
115 Eigen::AngleAxisd(0.05, Eigen::Vector3d(1., 0., 0.))));
transform::Rigid3d expected_pose_
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
sensor::PointCloud point_cloud_
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::CeresScanMatcherOptions options_
proto::CeresScanMatcherOptions CreateCeresScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::unique_ptr< CeresScanMatcher > ceres_scan_matcher_
std::vector< Eigen::Vector3f > PointCloud