precomputation_grid_3d_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
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 <random>
20 #include <tuple>
21 #include <vector>
22 
24 #include "gmock/gmock.h"
25 
26 namespace cartographer {
27 namespace mapping {
28 namespace scan_matching {
29 namespace {
30 
31 TEST(PrecomputedGridGenerator3DTest, TestAgainstNaiveAlgorithm) {
32  HybridGrid hybrid_grid(2.f);
33 
34  std::mt19937 rng(23847);
35  std::uniform_int_distribution<int> coordinate_distribution(-50, 49);
36  std::uniform_real_distribution<float> value_distribution(kMinProbability,
38  for (int i = 0; i < 1000; ++i) {
39  const auto x = coordinate_distribution(rng);
40  const auto y = coordinate_distribution(rng);
41  const auto z = coordinate_distribution(rng);
42  const Eigen::Array3i cell_index(x, y, z);
43  hybrid_grid.SetProbability(cell_index, value_distribution(rng));
44  }
45 
46  std::vector<PrecomputationGrid3D> precomputed_grids;
47  for (int depth = 0; depth <= 3; ++depth) {
48  if (depth == 0) {
49  precomputed_grids.push_back(ConvertToPrecomputationGrid(hybrid_grid));
50  } else {
51  precomputed_grids.push_back(
52  PrecomputeGrid(precomputed_grids.back(), false,
53  (1 << (depth - 1)) * Eigen::Array3i::Ones()));
54  }
55  const int width = 1 << depth;
56  for (int i = 0; i < 100; ++i) {
57  const auto x = coordinate_distribution(rng);
58  const auto y = coordinate_distribution(rng);
59  const auto z = coordinate_distribution(rng);
60  float max_probability = 0.;
61  for (int dx = 0; dx < width; ++dx) {
62  for (int dy = 0; dy < width; ++dy) {
63  for (int dz = 0; dz < width; ++dz) {
64  max_probability = std::max(
65  max_probability, hybrid_grid.GetProbability(
66  Eigen::Array3i(x + dx, y + dy, z + dz)));
67  }
68  }
69  }
70 
71  EXPECT_NEAR(max_probability,
73  precomputed_grids.back().value(Eigen::Array3i(x, y, z))),
74  1e-2);
75  }
76  }
77 }
78 
79 } // namespace
80 } // namespace scan_matching
81 } // namespace mapping
82 } // namespace cartographer
PrecomputationGrid3D ConvertToPrecomputationGrid(const HybridGrid &hybrid_grid)
constexpr float kMinProbability
float max_probability
Definition: submap_3d.cc:35
constexpr float kMaxProbability
PrecomputationGrid3D PrecomputeGrid(const PrecomputationGrid3D &grid, const bool half_resolution, const Eigen::Array3i &shift)
TEST(TrajectoryConnectivityStateTest, UnknownTrajectory)


cartographer
Author(s): The Cartographer Authors
autogenerated on Mon Feb 28 2022 22:00:58