30 #include "gtest/gtest.h" 34 namespace scan_matching {
37 TEST(PrecomputationGridTest, CorrectValues) {
40 std::mt19937 prng(42);
41 std::uniform_int_distribution<int> distribution(0, 255);
42 ProbabilityGrid probability_grid(
43 MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)));
44 std::vector<float> reusable_intermediate_grid;
45 PrecomputationGrid2D precomputation_grid_dummy(
46 probability_grid, probability_grid.limits().cell_limits(), 1,
47 &reusable_intermediate_grid);
48 for (
const Eigen::Array2i& xy_index :
49 XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
50 probability_grid.SetProbability(
51 xy_index, precomputation_grid_dummy.ToScore(distribution(prng)));
54 reusable_intermediate_grid.clear();
55 for (
const int width : {1, 2, 3, 8}) {
56 PrecomputationGrid2D precomputation_grid(
57 probability_grid, probability_grid.limits().cell_limits(), width,
58 &reusable_intermediate_grid);
59 for (
const Eigen::Array2i& xy_index :
60 XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
61 float max_score = -std::numeric_limits<float>::infinity();
62 for (
int y = 0; y != width; ++y) {
63 for (
int x = 0; x != width; ++x) {
64 max_score = std::max<float>(
66 probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
71 precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)),
77 TEST(PrecomputationGridTest, TinyProbabilityGrid) {
78 std::mt19937 prng(42);
79 std::uniform_int_distribution<int> distribution(0, 255);
80 ProbabilityGrid probability_grid(
81 MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
82 std::vector<float> reusable_intermediate_grid;
83 PrecomputationGrid2D precomputation_grid_dummy(
84 probability_grid, probability_grid.limits().cell_limits(), 1,
85 &reusable_intermediate_grid);
86 for (
const Eigen::Array2i& xy_index :
87 XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
88 probability_grid.SetProbability(
89 xy_index, precomputation_grid_dummy.ToScore(distribution(prng)));
92 reusable_intermediate_grid.clear();
93 for (
const int width : {1, 2, 3, 8, 200}) {
94 PrecomputationGrid2D precomputation_grid(
95 probability_grid, probability_grid.limits().cell_limits(), width,
96 &reusable_intermediate_grid);
97 for (
const Eigen::Array2i& xy_index :
98 XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
99 float max_score = -std::numeric_limits<float>::infinity();
100 for (
int y = 0; y != width; ++y) {
101 for (
int x = 0; x != width; ++x) {
102 max_score = std::max<float>(
104 probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
109 precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)),
115 proto::FastCorrelativeScanMatcherOptions2D
116 CreateFastCorrelativeScanMatcherTestOptions2D(
117 const int branch_and_bound_depth) {
118 auto parameter_dictionary =
121 linear_search_window = 3., 122 angular_search_window = 1., 123 branch_and_bound_depth = )text" + 124 std::to_string(branch_and_bound_depth) + "}");
128 mapping::proto::ProbabilityGridRangeDataInserterOptions2D
129 CreateRangeDataInserterTestOptions2D() {
132 insert_free_space = true, 133 hit_probability = 0.7, 134 miss_probability = 0.4, 137 parameter_dictionary.get());
140 TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
141 std::mt19937 prng(42);
142 std::uniform_real_distribution<float> distribution(-1.f, 1.f);
143 ProbabilityGridRangeDataInserter2D range_data_inserter(
144 CreateRangeDataInserterTestOptions2D());
145 constexpr
float kMinScore = 0.1f;
146 const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
149 point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
150 point_cloud.emplace_back(-2.f, 0.5f, 0.f);
151 point_cloud.emplace_back(0.f, -0.5f, 0.f);
152 point_cloud.emplace_back(0.5f, -1.6f, 0.f);
153 point_cloud.emplace_back(2.5f, 0.5f, 0.f);
154 point_cloud.emplace_back(2.5f, 1.7f, 0.f);
156 for (
int i = 0; i != 50; ++i) {
158 {2. * distribution(prng), 2. * distribution(prng)},
159 0.5 * distribution(prng));
161 ProbabilityGrid probability_grid(
162 MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
163 range_data_inserter.Insert(
165 Eigen::Vector3f(expected_pose.translation().x(),
166 expected_pose.translation().y(), 0.f),
171 probability_grid.FinishUpdate();
173 FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
177 EXPECT_TRUE(fast_correlative_scan_matcher.Match(
180 EXPECT_LT(kMinScore, score);
181 EXPECT_THAT(expected_pose,
182 transform::IsNearly(pose_estimate.cast<
float>(), 0.03f))
188 TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
189 std::mt19937 prng(42);
190 std::uniform_real_distribution<float> distribution(-1.f, 1.f);
191 ProbabilityGridRangeDataInserter2D range_data_inserter(
192 CreateRangeDataInserterTestOptions2D());
193 constexpr
float kMinScore = 0.1f;
194 const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
197 unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
198 unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f);
199 unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f);
200 unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f);
201 unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f);
202 unperturbed_point_cloud.emplace_back(2.f, 1.8f, 0.f);
204 for (
int i = 0; i != 20; ++i) {
206 {10. * distribution(prng), 10. * distribution(prng)},
207 1.6 * distribution(prng));
212 0.5 * distribution(prng)) *
213 perturbation.inverse();
215 ProbabilityGrid probability_grid(
216 MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
217 range_data_inserter.Insert(
224 probability_grid.FinishUpdate();
226 FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
230 EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
231 point_cloud, kMinScore, &score, &pose_estimate));
232 EXPECT_LT(kMinScore, score);
233 EXPECT_THAT(expected_pose,
234 transform::IsNearly(pose_estimate.cast<
float>(), 0.03f))
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
proto::FastCorrelativeScanMatcherOptions2D CreateFastCorrelativeScanMatcherOptions2D(common::LuaParameterDictionary *const parameter_dictionary)
proto::ProbabilityGridRangeDataInserterOptions2D CreateProbabilityGridRangeDataInserterOptions2D(common::LuaParameterDictionary *parameter_dictionary)
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const std::string &code)
std::vector< Eigen::Vector3f > PointCloud
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)