range_data_inserter_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 <memory>
20 #include <vector>
21 
23 #include "gmock/gmock.h"
24 
25 namespace cartographer {
26 namespace mapping {
27 namespace {
28 
29 class RangeDataInserter3DTest : public ::testing::Test {
30  protected:
31  RangeDataInserter3DTest() : hybrid_grid_(1.f) {
32  auto parameter_dictionary = common::MakeDictionary(
33  "return { "
34  "hit_probability = 0.7, "
35  "miss_probability = 0.4, "
36  "num_free_space_voxels = 1000, "
37  "}");
38  options_ = CreateRangeDataInserterOptions3D(parameter_dictionary.get());
39  range_data_inserter_.reset(new RangeDataInserter3D(options_));
40  }
41 
42  void InsertPointCloud() {
43  const Eigen::Vector3f origin = Eigen::Vector3f(0.f, 0.f, -4.f);
44  sensor::PointCloud returns = {
45  {-3.f, -1.f, 4.f}, {-2.f, 0.f, 4.f}, {-1.f, 1.f, 4.f}, {0.f, 2.f, 4.f}};
46  range_data_inserter_->Insert(sensor::RangeData{origin, returns, {}},
47  &hybrid_grid_);
48  }
49 
50  float GetProbability(float x, float y, float z) const {
51  return hybrid_grid_.GetProbability(
52  hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
53  }
54 
55  float IsKnown(float x, float y, float z) const {
56  return hybrid_grid_.IsKnown(
57  hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
58  }
59 
60  const proto::RangeDataInserterOptions3D& options() const { return options_; }
61 
62  private:
63  HybridGrid hybrid_grid_;
64  std::unique_ptr<RangeDataInserter3D> range_data_inserter_;
65  proto::RangeDataInserterOptions3D options_;
66 };
67 
68 TEST_F(RangeDataInserter3DTest, InsertPointCloud) {
69  InsertPointCloud();
70  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -4.f),
71  1e-4);
72  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
73  1e-4);
74  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -2.f),
75  1e-4);
76  for (int x = -4; x <= 4; ++x) {
77  for (int y = -4; y <= 4; ++y) {
78  if (x < -3 || x > 0 || y != x + 2) {
79  EXPECT_FALSE(IsKnown(x, y, 4.f));
80  } else {
81  EXPECT_NEAR(options().hit_probability(), GetProbability(x, y, 4.f),
82  1e-4);
83  }
84  }
85  }
86 }
87 
88 TEST_F(RangeDataInserter3DTest, ProbabilityProgression) {
89  InsertPointCloud();
90  EXPECT_NEAR(options().hit_probability(), GetProbability(-2.f, 0.f, 4.f),
91  1e-4);
92  EXPECT_NEAR(options().miss_probability(), GetProbability(-2.f, 0.f, 3.f),
93  1e-4);
94  EXPECT_NEAR(options().miss_probability(), GetProbability(0.f, 0.f, -3.f),
95  1e-4);
96 
97  for (int i = 0; i < 1000; ++i) {
98  InsertPointCloud();
99  }
100  EXPECT_NEAR(kMaxProbability, GetProbability(-2.f, 0.f, 4.f), 1e-3);
101  EXPECT_NEAR(kMinProbability, GetProbability(-2.f, 0.f, 3.f), 1e-3);
102  EXPECT_NEAR(kMinProbability, GetProbability(0.f, 0.f, -3.f), 1e-3);
103 }
104 
105 } // namespace
106 } // namespace mapping
107 } // namespace cartographer
proto::RangeDataInserterOptions3D options_
proto::RangeDataInserterOptions3D CreateRangeDataInserterOptions3D(common::LuaParameterDictionary *parameter_dictionary)
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const std::string &code)
constexpr float kMinProbability
constexpr float kMaxProbability
std::vector< Eigen::Vector3f > PointCloud
Definition: point_cloud.h:32
HybridGrid hybrid_grid_
std::unique_ptr< RangeDataInserter3D > range_data_inserter_


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