Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "cartographer/mapping/internal/3d/scan_matching/precomputation_grid_3d.h"
00018
00019 #include <random>
00020 #include <tuple>
00021 #include <vector>
00022
00023 #include "cartographer/mapping/3d/hybrid_grid.h"
00024 #include "gmock/gmock.h"
00025
00026 namespace cartographer {
00027 namespace mapping {
00028 namespace scan_matching {
00029 namespace {
00030
00031 TEST(PrecomputedGridGenerator3DTest, TestAgainstNaiveAlgorithm) {
00032 HybridGrid hybrid_grid(2.f);
00033
00034 std::mt19937 rng(23847);
00035 std::uniform_int_distribution<int> coordinate_distribution(-50, 49);
00036 std::uniform_real_distribution<float> value_distribution(kMinProbability,
00037 kMaxProbability);
00038 for (int i = 0; i < 1000; ++i) {
00039 const auto x = coordinate_distribution(rng);
00040 const auto y = coordinate_distribution(rng);
00041 const auto z = coordinate_distribution(rng);
00042 const Eigen::Array3i cell_index(x, y, z);
00043 hybrid_grid.SetProbability(cell_index, value_distribution(rng));
00044 }
00045
00046 std::vector<PrecomputationGrid3D> precomputed_grids;
00047 for (int depth = 0; depth <= 3; ++depth) {
00048 if (depth == 0) {
00049 precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid));
00050 } else {
00051 precomputed_grids.push_back(
00052 PrecomputeGrid(precomputed_grids.back(), false,
00053 (1 << (depth - 1)) * Eigen::Array3i::Ones()));
00054 }
00055 const int width = 1 << depth;
00056 for (int i = 0; i < 100; ++i) {
00057 const auto x = coordinate_distribution(rng);
00058 const auto y = coordinate_distribution(rng);
00059 const auto z = coordinate_distribution(rng);
00060 float max_probability = 0.;
00061 for (int dx = 0; dx < width; ++dx) {
00062 for (int dy = 0; dy < width; ++dy) {
00063 for (int dz = 0; dz < width; ++dz) {
00064 max_probability = std::max(
00065 max_probability, hybrid_grid.GetProbability(
00066 Eigen::Array3i(x + dx, y + dy, z + dz)));
00067 }
00068 }
00069 }
00070
00071 EXPECT_NEAR(max_probability,
00072 PrecomputationGrid3D::ToProbability(
00073 precomputed_grids.back().value(Eigen::Array3i(x, y, z))),
00074 1e-2);
00075 }
00076 }
00077 }
00078
00079 }
00080 }
00081 }
00082 }