mapping_2d/scan_matching/fast_correlative_scan_matcher_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
18
19 #include <algorithm>
20 #include <cmath>
21 #include <limits>
22 #include <random>
23 #include <string>
24
27
30 #include "gtest/gtest.h"
31
32 namespace cartographer {
33 namespace mapping_2d {
34 namespace scan_matching {
35 namespace {
36
37 TEST(PrecomputationGridTest, CorrectValues) {
38  // Create a probability grid with random values that can be exactly
39  // represented by uint8 values.
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  probability_grid.StartUpdate();
45  for (const Eigen::Array2i& xy_index :
46  XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
47  probability_grid.SetProbability(
48  xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
49  }
50
51  std::vector<float> reusable_intermediate_grid;
52  for (const int width : {1, 2, 3, 8}) {
53  PrecomputationGrid precomputation_grid(
54  probability_grid, probability_grid.limits().cell_limits(), width,
55  &reusable_intermediate_grid);
56  for (const Eigen::Array2i& xy_index :
57  XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
58  float max_score = -std::numeric_limits<float>::infinity();
59  for (int y = 0; y != width; ++y) {
60  for (int x = 0; x != width; ++x) {
61  max_score = std::max<float>(
62  max_score,
63  probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
64  }
65  }
66  EXPECT_NEAR(max_score,
68  precomputation_grid.GetValue(xy_index)),
69  1e-4);
70  }
71  }
72 }
73
74 TEST(PrecomputationGridTest, TinyProbabilityGrid) {
75  std::mt19937 prng(42);
76  std::uniform_int_distribution<int> distribution(0, 255);
77  ProbabilityGrid probability_grid(
78  MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)));
79  probability_grid.StartUpdate();
80  for (const Eigen::Array2i& xy_index :
81  XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
82  probability_grid.SetProbability(
83  xy_index, PrecomputationGrid::ToProbability(distribution(prng)));
84  }
85
86  std::vector<float> reusable_intermediate_grid;
87  for (const int width : {1, 2, 3, 8, 200}) {
88  PrecomputationGrid precomputation_grid(
89  probability_grid, probability_grid.limits().cell_limits(), width,
90  &reusable_intermediate_grid);
91  for (const Eigen::Array2i& xy_index :
92  XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
93  float max_score = -std::numeric_limits<float>::infinity();
94  for (int y = 0; y != width; ++y) {
95  for (int x = 0; x != width; ++x) {
96  max_score = std::max<float>(
97  max_score,
98  probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
99  }
100  }
101  EXPECT_NEAR(max_score,
103  precomputation_grid.GetValue(xy_index)),
104  1e-4);
105  }
106  }
107 }
108
109 proto::FastCorrelativeScanMatcherOptions
110 CreateFastCorrelativeScanMatcherTestOptions(const int branch_and_bound_depth) {
111  auto parameter_dictionary =
112  common::MakeDictionary(R"text(
113  return {
114  linear_search_window = 3.,
115  angular_search_window = 1.,
116  branch_and_bound_depth = )text" +
117  std::to_string(branch_and_bound_depth) + "}");
118  return CreateFastCorrelativeScanMatcherOptions(parameter_dictionary.get());
119 }
120
121 mapping_2d::proto::RangeDataInserterOptions
122 CreateRangeDataInserterTestOptions() {
123  auto parameter_dictionary = common::MakeDictionary(R"text(
124  return {
125  insert_free_space = true,
126  hit_probability = 0.7,
127  miss_probability = 0.4,
128  })text");
129  return mapping_2d::CreateRangeDataInserterOptions(parameter_dictionary.get());
130 }
131
132 TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
133  std::mt19937 prng(42);
134  std::uniform_real_distribution<float> distribution(-1.f, 1.f);
135  RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
136  constexpr float kMinScore = 0.1f;
137  const auto options = CreateFastCorrelativeScanMatcherTestOptions(3);
138
139  sensor::PointCloud point_cloud;
140  point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
141  point_cloud.emplace_back(-2.f, 0.5f, 0.f);
142  point_cloud.emplace_back(0.f, -0.5f, 0.f);
143  point_cloud.emplace_back(0.5f, -1.6f, 0.f);
144  point_cloud.emplace_back(2.5f, 0.5f, 0.f);
145  point_cloud.emplace_back(2.5f, 1.7f, 0.f);
146
147  for (int i = 0; i != 50; ++i) {
148  const transform::Rigid2f expected_pose(
149  {2. * distribution(prng), 2. * distribution(prng)},
150  0.5 * distribution(prng));
151
152  ProbabilityGrid probability_grid(
153  MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
154  probability_grid.StartUpdate();
155  range_data_inserter.Insert(
156  sensor::RangeData{
157  Eigen::Vector3f(expected_pose.translation().x(),
158  expected_pose.translation().y(), 0.f),
160  point_cloud, transform::Embed3D(expected_pose.cast<float>())),
161  {}},
162  &probability_grid);
163
164  FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
165  options);
166  transform::Rigid2d pose_estimate;
167  float score;
168  EXPECT_TRUE(fast_correlative_scan_matcher.Match(
169  transform::Rigid2d::Identity(), point_cloud, kMinScore, &score,
170  &pose_estimate));
171  EXPECT_LT(kMinScore, score);
172  EXPECT_THAT(expected_pose,
173  transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
174  << "Actual: " << transform::ToProto(pose_estimate).DebugString()
175  << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
176  }
177 }
178
179 TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
180  std::mt19937 prng(42);
181  std::uniform_real_distribution<float> distribution(-1.f, 1.f);
182  RangeDataInserter range_data_inserter(CreateRangeDataInserterTestOptions());
183  constexpr float kMinScore = 0.1f;
184  const auto options = CreateFastCorrelativeScanMatcherTestOptions(6);
185
186  sensor::PointCloud unperturbed_point_cloud;
187  unperturbed_point_cloud.emplace_back(-2.5f, 0.5f, 0.f);
188  unperturbed_point_cloud.emplace_back(-2.25f, 0.5f, 0.f);
189  unperturbed_point_cloud.emplace_back(0.f, 0.5f, 0.f);
190  unperturbed_point_cloud.emplace_back(0.25f, 1.6f, 0.f);
191  unperturbed_point_cloud.emplace_back(2.5f, 0.5f, 0.f);
192  unperturbed_point_cloud.emplace_back(2.0f, 1.8f, 0.f);
193
194  for (int i = 0; i != 20; ++i) {
195  const transform::Rigid2f perturbation(
196  {10. * distribution(prng), 10. * distribution(prng)},
197  1.6 * distribution(prng));
199  unperturbed_point_cloud, transform::Embed3D(perturbation));
200  const transform::Rigid2f expected_pose =
201  transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)},
202  0.5 * distribution(prng)) *
203  perturbation.inverse();
204
205  ProbabilityGrid probability_grid(
206  MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)));
207  probability_grid.StartUpdate();
208  range_data_inserter.Insert(
209  sensor::RangeData{
210  transform::Embed3D(expected_pose * perturbation).translation(),
211  sensor::TransformPointCloud(point_cloud,
212  transform::Embed3D(expected_pose)),
213  {}},
214  &probability_grid);
215
216  FastCorrelativeScanMatcher fast_correlative_scan_matcher(probability_grid,
217  options);
218  transform::Rigid2d pose_estimate;
219  float score;
220  EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
221  point_cloud, kMinScore, &score, &pose_estimate));
222  EXPECT_LT(kMinScore, score);
223  EXPECT_THAT(expected_pose,
224  transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
225  << "Actual: " << transform::ToProto(pose_estimate).DebugString()
226  << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
227  }
228 }
229
230 } // namespace
231 } // namespace scan_matching
232 } // namespace mapping_2d
233 } // namespace cartographer
PointCloud TransformPointCloud(const PointCloud &point_cloud, const transform::Rigid3f &transform)
Definition: point_cloud.cc:25
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
static Rigid2< FloatType > Identity()
Rigid2< double > Rigid2d
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::Rigid2d ToProto(const transform::Rigid2d &transform)
Definition: transform.cc:44
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *const parameter_dictionary)
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:30
proto::FastCorrelativeScanMatcherOptions CreateFastCorrelativeScanMatcherOptions(common::LuaParameterDictionary *const parameter_dictionary)

cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:38