00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/2d/scan_matching/fast_correlative_scan_matcher_2d.h"
00018
00019 #include <algorithm>
00020 #include <cmath>
00021 #include <limits>
00022 #include <random>
00023 #include <string>
00024
00025 #include "cartographer/common/lua_parameter_dictionary_test_helpers.h"
00026 #include "cartographer/mapping/2d/probability_grid.h"
00027 #include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"
00028 #include "cartographer/transform/rigid_transform_test_helpers.h"
00029 #include "cartographer/transform/transform.h"
00030 #include "gtest/gtest.h"
00031
00032 namespace cartographer {
00033 namespace mapping {
00034 namespace scan_matching {
00035 namespace {
00036
00037 TEST(PrecomputationGridTest, CorrectValues) {
00038
00039
00040 std::mt19937 prng(42);
00041 std::uniform_int_distribution<int> distribution(0, 255);
00042 ValueConversionTables conversion_tables;
00043 ProbabilityGrid probability_grid(
00044 MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(250, 250)),
00045 &conversion_tables);
00046 std::vector<float> reusable_intermediate_grid;
00047 PrecomputationGrid2D precomputation_grid_dummy(
00048 probability_grid, probability_grid.limits().cell_limits(), 1,
00049 &reusable_intermediate_grid);
00050 for (const Eigen::Array2i& xy_index :
00051 XYIndexRangeIterator(Eigen::Array2i(50, 50), Eigen::Array2i(249, 249))) {
00052 probability_grid.SetProbability(
00053 xy_index, precomputation_grid_dummy.ToScore(distribution(prng)));
00054 }
00055
00056 reusable_intermediate_grid.clear();
00057 for (const int width : {1, 2, 3, 8}) {
00058 PrecomputationGrid2D precomputation_grid(
00059 probability_grid, probability_grid.limits().cell_limits(), width,
00060 &reusable_intermediate_grid);
00061 for (const Eigen::Array2i& xy_index :
00062 XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
00063 float max_score = -std::numeric_limits<float>::infinity();
00064 for (int y = 0; y != width; ++y) {
00065 for (int x = 0; x != width; ++x) {
00066 max_score = std::max<float>(
00067 max_score,
00068 probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
00069 }
00070 }
00071 EXPECT_NEAR(
00072 max_score,
00073 precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)),
00074 1e-4);
00075 }
00076 }
00077 }
00078
00079 TEST(PrecomputationGridTest, TinyProbabilityGrid) {
00080 std::mt19937 prng(42);
00081 std::uniform_int_distribution<int> distribution(0, 255);
00082 ValueConversionTables conversion_tables;
00083 ProbabilityGrid probability_grid(
00084 MapLimits(0.05, Eigen::Vector2d(0.1, 0.1), CellLimits(4, 4)),
00085 &conversion_tables);
00086 std::vector<float> reusable_intermediate_grid;
00087 PrecomputationGrid2D precomputation_grid_dummy(
00088 probability_grid, probability_grid.limits().cell_limits(), 1,
00089 &reusable_intermediate_grid);
00090 for (const Eigen::Array2i& xy_index :
00091 XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
00092 probability_grid.SetProbability(
00093 xy_index, precomputation_grid_dummy.ToScore(distribution(prng)));
00094 }
00095
00096 reusable_intermediate_grid.clear();
00097 for (const int width : {1, 2, 3, 8, 200}) {
00098 PrecomputationGrid2D precomputation_grid(
00099 probability_grid, probability_grid.limits().cell_limits(), width,
00100 &reusable_intermediate_grid);
00101 for (const Eigen::Array2i& xy_index :
00102 XYIndexRangeIterator(probability_grid.limits().cell_limits())) {
00103 float max_score = -std::numeric_limits<float>::infinity();
00104 for (int y = 0; y != width; ++y) {
00105 for (int x = 0; x != width; ++x) {
00106 max_score = std::max<float>(
00107 max_score,
00108 probability_grid.GetProbability(xy_index + Eigen::Array2i(x, y)));
00109 }
00110 }
00111 EXPECT_NEAR(
00112 max_score,
00113 precomputation_grid.ToScore(precomputation_grid.GetValue(xy_index)),
00114 1e-4);
00115 }
00116 }
00117 }
00118
00119 proto::FastCorrelativeScanMatcherOptions2D
00120 CreateFastCorrelativeScanMatcherTestOptions2D(
00121 const int branch_and_bound_depth) {
00122 auto parameter_dictionary =
00123 common::MakeDictionary(R"text(
00124 return {
00125 linear_search_window = 3.,
00126 angular_search_window = 1.,
00127 branch_and_bound_depth = )text" +
00128 std::to_string(branch_and_bound_depth) + "}");
00129 return CreateFastCorrelativeScanMatcherOptions2D(parameter_dictionary.get());
00130 }
00131
00132 mapping::proto::ProbabilityGridRangeDataInserterOptions2D
00133 CreateRangeDataInserterTestOptions2D() {
00134 auto parameter_dictionary = common::MakeDictionary(R"text(
00135 return {
00136 insert_free_space = true,
00137 hit_probability = 0.7,
00138 miss_probability = 0.4,
00139 })text");
00140 return mapping::CreateProbabilityGridRangeDataInserterOptions2D(
00141 parameter_dictionary.get());
00142 }
00143
00144 TEST(FastCorrelativeScanMatcherTest, CorrectPose) {
00145 std::mt19937 prng(42);
00146 std::uniform_real_distribution<float> distribution(-1.f, 1.f);
00147 ProbabilityGridRangeDataInserter2D range_data_inserter(
00148 CreateRangeDataInserterTestOptions2D());
00149 constexpr float kMinScore = 0.1f;
00150 const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(3);
00151
00152 sensor::PointCloud point_cloud;
00153 point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
00154 point_cloud.push_back({Eigen::Vector3f{-2.f, 0.5f, 0.f}});
00155 point_cloud.push_back({Eigen::Vector3f{0.f, -0.5f, 0.f}});
00156 point_cloud.push_back({Eigen::Vector3f{0.5f, -1.6f, 0.f}});
00157 point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
00158 point_cloud.push_back({Eigen::Vector3f{2.5f, 1.7f, 0.f}});
00159
00160 for (int i = 0; i != 50; ++i) {
00161 const transform::Rigid2f expected_pose(
00162 {2. * distribution(prng), 2. * distribution(prng)},
00163 0.5 * distribution(prng));
00164
00165 ValueConversionTables conversion_tables;
00166 ProbabilityGrid probability_grid(
00167 MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)),
00168 &conversion_tables);
00169 range_data_inserter.Insert(
00170 sensor::RangeData{
00171 Eigen::Vector3f(expected_pose.translation().x(),
00172 expected_pose.translation().y(), 0.f),
00173 sensor::TransformPointCloud(
00174 point_cloud, transform::Embed3D(expected_pose.cast<float>())),
00175 {}},
00176 &probability_grid);
00177 probability_grid.FinishUpdate();
00178
00179 FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
00180 options);
00181 transform::Rigid2d pose_estimate;
00182 float score;
00183 EXPECT_TRUE(fast_correlative_scan_matcher.Match(
00184 transform::Rigid2d::Identity(), point_cloud, kMinScore, &score,
00185 &pose_estimate));
00186 EXPECT_LT(kMinScore, score);
00187 EXPECT_THAT(expected_pose,
00188 transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
00189 << "Actual: " << transform::ToProto(pose_estimate).DebugString()
00190 << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
00191 }
00192 }
00193
00194 TEST(FastCorrelativeScanMatcherTest, FullSubmapMatching) {
00195 std::mt19937 prng(42);
00196 std::uniform_real_distribution<float> distribution(-1.f, 1.f);
00197 ProbabilityGridRangeDataInserter2D range_data_inserter(
00198 CreateRangeDataInserterTestOptions2D());
00199 constexpr float kMinScore = 0.1f;
00200 const auto options = CreateFastCorrelativeScanMatcherTestOptions2D(6);
00201
00202 sensor::PointCloud unperturbed_point_cloud;
00203 unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.5f, 0.5f, 0.f}});
00204 unperturbed_point_cloud.push_back({Eigen::Vector3f{-2.25f, 0.5f, 0.f}});
00205 unperturbed_point_cloud.push_back({Eigen::Vector3f{0.f, 0.5f, 0.f}});
00206 unperturbed_point_cloud.push_back({Eigen::Vector3f{0.25f, 1.6f, 0.f}});
00207 unperturbed_point_cloud.push_back({Eigen::Vector3f{2.5f, 0.5f, 0.f}});
00208 unperturbed_point_cloud.push_back({Eigen::Vector3f{2.f, 1.8f, 0.f}});
00209
00210 for (int i = 0; i != 20; ++i) {
00211 const transform::Rigid2f perturbation(
00212 {10. * distribution(prng), 10. * distribution(prng)},
00213 1.6 * distribution(prng));
00214 const sensor::PointCloud point_cloud = sensor::TransformPointCloud(
00215 unperturbed_point_cloud, transform::Embed3D(perturbation));
00216 const transform::Rigid2f expected_pose =
00217 transform::Rigid2f({2. * distribution(prng), 2. * distribution(prng)},
00218 0.5 * distribution(prng)) *
00219 perturbation.inverse();
00220
00221 ValueConversionTables conversion_tables;
00222 ProbabilityGrid probability_grid(
00223 MapLimits(0.05, Eigen::Vector2d(5., 5.), CellLimits(200, 200)),
00224 &conversion_tables);
00225 range_data_inserter.Insert(
00226 sensor::RangeData{
00227 transform::Embed3D(expected_pose * perturbation).translation(),
00228 sensor::TransformPointCloud(point_cloud,
00229 transform::Embed3D(expected_pose)),
00230 {}},
00231 &probability_grid);
00232 probability_grid.FinishUpdate();
00233
00234 FastCorrelativeScanMatcher2D fast_correlative_scan_matcher(probability_grid,
00235 options);
00236 transform::Rigid2d pose_estimate;
00237 float score;
00238 EXPECT_TRUE(fast_correlative_scan_matcher.MatchFullSubmap(
00239 point_cloud, kMinScore, &score, &pose_estimate));
00240 EXPECT_LT(kMinScore, score);
00241 EXPECT_THAT(expected_pose,
00242 transform::IsNearly(pose_estimate.cast<float>(), 0.03f))
00243 << "Actual: " << transform::ToProto(pose_estimate).DebugString()
00244 << "\nExpected: " << transform::ToProto(expected_pose).DebugString();
00245 }
00246 }
00247
00248 }
00249 }
00250 }
00251 }