interpolated_grid_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 "Eigen/Core"
21 #include "gtest/gtest.h"
22 
23 namespace cartographer {
24 namespace mapping {
25 namespace scan_matching {
26 namespace {
27 
28 class InterpolatedGridTest : public ::testing::Test {
29  protected:
30  InterpolatedGridTest()
32  for (const Eigen::Vector3f& point :
33  {Eigen::Vector3f(-3.f, 2.f, 0.f), Eigen::Vector3f(-4.f, 2.f, 0.f),
34  Eigen::Vector3f(-5.f, 2.f, 0.f), Eigen::Vector3f(-6.f, 2.f, 0.f),
35  Eigen::Vector3f(-6.f, 3.f, 1.f), Eigen::Vector3f(-6.f, 4.f, 2.f),
36  Eigen::Vector3f(-7.f, 3.f, 1.f)}) {
37  hybrid_grid_.SetProbability(hybrid_grid_.GetCellIndex(point), 1.);
38  }
39  }
40 
41  float GetHybridGridProbability(float x, float y, float z) const {
42  return hybrid_grid_.GetProbability(
43  hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
44  }
45 
46  HybridGrid hybrid_grid_;
47  InterpolatedGrid interpolated_grid_;
48 };
49 
50 TEST_F(InterpolatedGridTest, InterpolatesGridPoints) {
51  for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) {
52  for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) {
53  for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) {
54  EXPECT_NEAR(GetHybridGridProbability(x, y, z),
55  interpolated_grid_.GetProbability(x, y, z), 1e-6);
56  }
57  }
58  }
59 }
60 
61 TEST_F(InterpolatedGridTest, MonotonicBehaviorBetweenGridPointsInX) {
62  const double kSampleStep = hybrid_grid_.resolution() / 10.;
63  for (double z = -1.; z < 3.; z += hybrid_grid_.resolution()) {
64  for (double y = 1.; y < 5.; y += hybrid_grid_.resolution()) {
65  for (double x = -8.; x < -2.; x += hybrid_grid_.resolution()) {
66  const float start_probability = GetHybridGridProbability(x, y, z);
67  const float next_probability =
68  GetHybridGridProbability(x + hybrid_grid_.resolution(), y, z);
69  const float grid_difference = next_probability - start_probability;
70  if (std::abs(grid_difference) < 1e-6f) {
71  continue;
72  }
73  for (double sample = kSampleStep;
74  sample < hybrid_grid_.resolution() - 2 * kSampleStep;
75  sample += kSampleStep) {
76  EXPECT_LT(0., grid_difference * (interpolated_grid_.GetProbability(
77  x + sample + kSampleStep, y, z) -
78  interpolated_grid_.GetProbability(
79  x + sample, y, z)));
80  }
81  }
82  }
83  }
84 }
85 
86 } // namespace
87 } // namespace scan_matching
88 } // namespace mapping
89 } // namespace cartographer
HybridGrid hybrid_grid_
InterpolatedGrid interpolated_grid_


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