22 #include "Eigen/Geometry" 29 #include "gtest/gtest.h" 32 namespace mapping_2d {
33 namespace scan_matching {
36 class RealTimeCorrelativeScanMatcherTest :
public ::testing::Test {
38 RealTimeCorrelativeScanMatcherTest()
40 MapLimits(0.05,
Eigen::Vector2d(0.05, 0.25), CellLimits(6, 6))) {
44 "insert_free_space = true, " 45 "hit_probability = 0.7, " 46 "miss_probability = 0.4, " 60 sensor::RangeData{Eigen::Vector3f::Zero(),
point_cloud_, {}},
65 "linear_search_window = 0.6, " 66 "angular_search_window = 0.16, " 67 "translation_delta_cost_weight = 0., " 68 "rotation_delta_cost_weight = 0., " 71 common::make_unique<RealTimeCorrelativeScanMatcher>(
73 parameter_dictionary.get()));
80 std::unique_ptr<RealTimeCorrelativeScanMatcher>
84 TEST_F(RealTimeCorrelativeScanMatcherTest,
85 ScorePerfectHighResolutionCandidate) {
86 const std::vector<sensor::PointCloud> scans =
90 std::vector<Candidate> candidates;
91 candidates.emplace_back(0, 0, 0, SearchParameters(0, 0, 0., 0.));
95 EXPECT_EQ(0, candidates[0].scan_index);
96 EXPECT_EQ(0, candidates[0].x_index_offset);
97 EXPECT_EQ(0, candidates[0].y_index_offset);
99 EXPECT_NEAR(0.7, candidates[0].score, 1e-2);
102 TEST_F(RealTimeCorrelativeScanMatcherTest,
103 ScorePartiallyCorrectHighResolutionCandidate) {
104 const std::vector<sensor::PointCloud> scans =
108 std::vector<Candidate> candidates;
109 candidates.emplace_back(0, 0, 1, SearchParameters(0, 0, 0., 0.));
113 EXPECT_EQ(0, candidates[0].scan_index);
114 EXPECT_EQ(0, candidates[0].x_index_offset);
115 EXPECT_EQ(1, candidates[0].y_index_offset);
117 EXPECT_LT(0.7 * 3. / 7., candidates[0].score);
118 EXPECT_GT(0.7, candidates[0].score);
std::vector< DiscreteScan > DiscretizeScans(const MapLimits &map_limits, const std::vector< sensor::PointCloud > &scans, const Eigen::Translation2f &initial_translation)
std::unique_ptr< RealTimeCorrelativeScanMatcher > real_time_correlative_scan_matcher_
sensor::PointCloud point_cloud_
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *const parameter_dictionary)
proto::RealTimeCorrelativeScanMatcherOptions CreateRealTimeCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
std::unique_ptr< RangeDataInserter > range_data_inserter_
ProbabilityGrid probability_grid_
std::vector< sensor::PointCloud > GenerateRotatedScans(const sensor::PointCloud &point_cloud, const SearchParameters &search_parameters)