mapping_2d/range_data_inserter_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 
25 #include "gmock/gmock.h"
26 
27 namespace cartographer {
28 namespace mapping_2d {
29 namespace {
30 
31 class RangeDataInserterTest : public ::testing::Test {
32  protected:
33  RangeDataInserterTest()
35  MapLimits(1., Eigen::Vector2d(1., 5.), CellLimits(5, 5))) {
36  auto parameter_dictionary = common::MakeDictionary(
37  "return { "
38  "insert_free_space = true, "
39  "hit_probability = 0.7, "
40  "miss_probability = 0.4, "
41  "}");
42  options_ = CreateRangeDataInserterOptions(parameter_dictionary.get());
43  range_data_inserter_ = common::make_unique<RangeDataInserter>(options_);
44  }
45 
46  void InsertPointCloud() {
47  sensor::RangeData range_data;
48  range_data.returns.emplace_back(-3.5, 0.5, 0.f);
49  range_data.returns.emplace_back(-2.5, 1.5, 0.f);
50  range_data.returns.emplace_back(-1.5, 2.5, 0.f);
51  range_data.returns.emplace_back(-0.5, 3.5, 0.f);
52  range_data.origin.x() = -0.5;
53  range_data.origin.y() = 0.5;
54  probability_grid_.StartUpdate();
55  range_data_inserter_->Insert(range_data, &probability_grid_);
56  }
57 
58  ProbabilityGrid probability_grid_;
59  std::unique_ptr<RangeDataInserter> range_data_inserter_;
60  proto::RangeDataInserterOptions options_;
61 };
62 
63 TEST_F(RangeDataInserterTest, InsertPointCloud) {
64  InsertPointCloud();
65 
66  EXPECT_NEAR(1., probability_grid_.limits().max().x(), 1e-9);
67  EXPECT_NEAR(5., probability_grid_.limits().max().y(), 1e-9);
68 
69  const CellLimits& cell_limits = probability_grid_.limits().cell_limits();
70  EXPECT_EQ(5, cell_limits.num_x_cells);
71  EXPECT_EQ(5, cell_limits.num_y_cells);
72 
73  enum class State { UNKNOWN, MISS, HIT };
74  State expected_states[5][5] = {
75  {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN,
76  State::UNKNOWN},
77  {State::UNKNOWN, State::HIT, State::MISS, State::MISS, State::MISS},
78  {State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS, State::MISS},
79  {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::HIT, State::MISS},
80  {State::UNKNOWN, State::UNKNOWN, State::UNKNOWN, State::UNKNOWN,
81  State::HIT}};
82  for (int row = 0; row != 5; ++row) {
83  for (int column = 0; column != 5; ++column) {
84  Eigen::Array2i xy_index(row, column);
85  EXPECT_TRUE(probability_grid_.limits().Contains(xy_index));
86  switch (expected_states[column][row]) {
87  case State::UNKNOWN:
88  EXPECT_FALSE(probability_grid_.IsKnown(xy_index));
89  break;
90  case State::MISS:
91  EXPECT_NEAR(options_.miss_probability(),
92  probability_grid_.GetProbability(xy_index), 1e-4);
93  break;
94  case State::HIT:
95  EXPECT_NEAR(options_.hit_probability(),
96  probability_grid_.GetProbability(xy_index), 1e-4);
97  break;
98  }
99  }
100  }
101 }
102 
103 TEST_F(RangeDataInserterTest, ProbabilityProgression) {
104  InsertPointCloud();
105  EXPECT_NEAR(options_.hit_probability(),
106  probability_grid_.GetProbability(-3.5, 0.5), 1e-4);
107  EXPECT_NEAR(options_.miss_probability(),
108  probability_grid_.GetProbability(-2.5, 0.5), 1e-4);
109 
110  for (int i = 0; i < 1000; ++i) {
111  InsertPointCloud();
112  }
113  EXPECT_NEAR(mapping::kMaxProbability,
114  probability_grid_.GetProbability(-3.5, 0.5), 1e-3);
115  EXPECT_NEAR(mapping::kMinProbability,
116  probability_grid_.GetProbability(-2.5, 0.5), 1e-3);
117 }
118 
119 } // namespace
120 } // namespace mapping_2d
121 } // namespace cartographer
proto::RangeDataInserterOptions options_
constexpr float kMinProbability
constexpr float kMaxProbability
std::unique_ptr< LuaParameterDictionary > MakeDictionary(const string &code)
proto::RangeDataInserterOptions CreateRangeDataInserterOptions(common::LuaParameterDictionary *const parameter_dictionary)
ProbabilityGrid probability_grid_
std::unique_ptr< RangeDataInserter > range_data_inserter_


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:58