29 #include "gtest/gtest.h" 32 namespace mapping_3d {
33 namespace scan_matching {
36 class RealTimeCorrelativeScanMatcherTest :
public ::testing::Test {
38 RealTimeCorrelativeScanMatcherTest()
41 Eigen::Quaterniond::Identity()) {
42 for (
const auto& point :
43 {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
44 Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
45 Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
46 Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
54 linear_search_window = 0.3, 55 angular_search_window = math.rad(1.), 56 translation_delta_cost_weight = 1e-1, 57 rotation_delta_cost_weight = 1., 60 new RealTimeCorrelativeScanMatcher(
61 mapping_2d::scan_matching::
63 parameter_dictionary.get())));
71 LOG(INFO) <<
"Score: " << score;
78 std::unique_ptr<RealTimeCorrelativeScanMatcher>
82 TEST_F(RealTimeCorrelativeScanMatcherTest, PerfectEstimate) {
87 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongX) {
92 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongZ) {
97 TEST_F(RealTimeCorrelativeScanMatcherTest, AlongXYZ) {
102 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundX) {
104 Eigen::Vector3d(-1., 0., 0.),
105 Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(1., 0., 0.))));
108 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundY) {
110 Eigen::Vector3d(-1., 0., 0.),
111 Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 0.))));
114 TEST_F(RealTimeCorrelativeScanMatcherTest, RotationAroundYZ) {
116 Eigen::Vector3d(-1., 0., 0.),
117 Eigen::AngleAxisd(0.8 / 180. * M_PI, Eigen::Vector3d(0., 1., 1.))));
sensor::PointCloud point_cloud_
transform::Rigid3d expected_pose_
std::unique_ptr< RealTimeCorrelativeScanMatcher > real_time_correlative_scan_matcher_
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud