hybrid_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 
17 #include "cartographer/mapping_3d/hybrid_grid.h"
18 
19 #include <map>
20 #include <random>
21 #include <tuple>
22 
23 #include "gmock/gmock.h"
24 
25 namespace cartographer {
26 namespace mapping_3d {
27 namespace {
28 
29 TEST(HybridGridTest, ApplyOdds) {
30  HybridGrid hybrid_grid(1.f);
31 
32  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 0)));
33  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 0)));
34  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 0)));
35  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 0)));
36  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 0, 1)));
37  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(0, 1, 1)));
38  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 0, 1)));
39  EXPECT_FALSE(hybrid_grid.IsKnown(Eigen::Array3i(1, 1, 1)));
40 
41  hybrid_grid.SetProbability(Eigen::Array3i(1, 0, 1), 0.5f);
42 
43  hybrid_grid.StartUpdate();
44  hybrid_grid.ApplyLookupTable(
45  Eigen::Array3i(1, 0, 1),
46  mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
47  EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 0, 1)), 0.5f);
48 
49  hybrid_grid.SetProbability(Eigen::Array3i(0, 1, 0), 0.5f);
50 
51  hybrid_grid.StartUpdate();
52  hybrid_grid.ApplyLookupTable(
53  Eigen::Array3i(0, 1, 0),
54  mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.1f)));
55  EXPECT_LT(hybrid_grid.GetProbability(Eigen::Array3i(0, 1, 0)), 0.5f);
56 
57  // Tests adding odds to an unknown cell.
58  hybrid_grid.StartUpdate();
59  hybrid_grid.ApplyLookupTable(
60  Eigen::Array3i(1, 1, 1),
61  mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.42f)));
62  EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
63 
64  // Tests that further updates are ignored if StartUpdate() isn't called.
65  hybrid_grid.ApplyLookupTable(
66  Eigen::Array3i(1, 1, 1),
67  mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
68  EXPECT_NEAR(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f, 1e-4);
69  hybrid_grid.StartUpdate();
70  hybrid_grid.ApplyLookupTable(
71  Eigen::Array3i(1, 1, 1),
72  mapping::ComputeLookupTableToApplyOdds(mapping::Odds(0.9f)));
73  EXPECT_GT(hybrid_grid.GetProbability(Eigen::Array3i(1, 1, 1)), 0.42f);
74 }
75 
76 TEST(HybridGridTest, GetProbability) {
77  HybridGrid hybrid_grid(1.f);
78 
79  hybrid_grid.SetProbability(
80  hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f)),
81  mapping::kMaxProbability);
82  EXPECT_NEAR(hybrid_grid.GetProbability(
83  hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 1.f, 1.f))),
84  mapping::kMaxProbability, 1e-6);
85  for (const Eigen::Array3i& index :
86  {hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 2.f, 1.f)),
87  hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 1.f, 1.f)),
88  hybrid_grid.GetCellIndex(Eigen::Vector3f(1.f, 2.f, 1.f))}) {
89  EXPECT_FALSE(hybrid_grid.IsKnown(index));
90  }
91 }
92 
93 MATCHER_P(AllCwiseEqual, index, "") { return (arg == index).all(); }
94 
95 TEST(HybridGridTest, GetCellIndex) {
96  HybridGrid hybrid_grid(2.f);
97 
98  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 0.f, 0.f)),
99  AllCwiseEqual(Eigen::Array3i(0, 0, 0)));
100  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(0.f, 26.f, 10.f)),
101  AllCwiseEqual(Eigen::Array3i(0, 13, 5)));
102  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 0.f, 10.f)),
103  AllCwiseEqual(Eigen::Array3i(7, 0, 5)));
104  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(14.f, 26.f, 0.f)),
105  AllCwiseEqual(Eigen::Array3i(7, 13, 0)));
106 
107  // Check around the origin.
108  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(8.5f, 11.5f, 0.5f)),
109  AllCwiseEqual(Eigen::Array3i(4, 6, 0)));
110  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(7.5f, 12.5f, 1.5f)),
111  AllCwiseEqual(Eigen::Array3i(4, 6, 1)));
112  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(6.5f, 14.5f, 2.5f)),
113  AllCwiseEqual(Eigen::Array3i(3, 7, 1)));
114  EXPECT_THAT(hybrid_grid.GetCellIndex(Eigen::Vector3f(5.5f, 13.5f, 3.5f)),
115  AllCwiseEqual(Eigen::Array3i(3, 7, 2)));
116 }
117 
118 TEST(HybridGridTest, GetCenterOfCell) {
119  HybridGrid hybrid_grid(2.f);
120 
121  const Eigen::Array3i index(3, 2, 1);
122  const Eigen::Vector3f center = hybrid_grid.GetCenterOfCell(index);
123  EXPECT_NEAR(6.f, center.x(), 1e-6);
124  EXPECT_NEAR(4.f, center.y(), 1e-6);
125  EXPECT_NEAR(2.f, center.z(), 1e-6);
126  EXPECT_THAT(hybrid_grid.GetCellIndex(center), AllCwiseEqual(index));
127 }
128 
129 class RandomHybridGridTest : public ::testing::Test {
130  public:
131  RandomHybridGridTest() : hybrid_grid_(2.f), values_() {
132  std::mt19937 rng(1285120005);
133  std::uniform_real_distribution<float> value_distribution(
134  mapping::kMinProbability, mapping::kMaxProbability);
135  std::uniform_int_distribution<int> xyz_distribution(-3000, 2999);
136  for (int i = 0; i < 10000; ++i) {
137  const auto x = xyz_distribution(rng);
138  const auto y = xyz_distribution(rng);
139  const auto z = xyz_distribution(rng);
140  values_.emplace(std::make_tuple(x, y, z), value_distribution(rng));
141  }
142 
143  for (const auto& pair : values_) {
144  const Eigen::Array3i cell_index(std::get<0>(pair.first),
145  std::get<1>(pair.first),
146  std::get<2>(pair.first));
147  hybrid_grid_.SetProbability(cell_index, pair.second);
148  }
149  }
150 
151  protected:
152  HybridGrid hybrid_grid_;
153  using ValueMap = std::map<std::tuple<int, int, int>, float>;
154  ValueMap values_;
155 };
156 
157 TEST_F(RandomHybridGridTest, TestIteration) {
158  for (auto it = HybridGrid::Iterator(hybrid_grid_); !it.Done(); it.Next()) {
159  const Eigen::Array3i cell_index = it.GetCellIndex();
160  const float iterator_probability =
161  mapping::ValueToProbability(it.GetValue());
162  EXPECT_EQ(iterator_probability, hybrid_grid_.GetProbability(cell_index));
163  const std::tuple<int, int, int> key =
164  std::make_tuple(cell_index[0], cell_index[1], cell_index[2]);
165  EXPECT_TRUE(values_.count(key));
166  EXPECT_NEAR(values_[key], iterator_probability, 1e-4);
167  values_.erase(key);
168  }
169 
170  // Test that range based loop is equivalent to using the iterator.
171  auto it = HybridGrid::Iterator(hybrid_grid_);
172  for (const auto& cell : hybrid_grid_) {
173  ASSERT_FALSE(it.Done());
174  EXPECT_THAT(cell.first, AllCwiseEqual(it.GetCellIndex()));
175  EXPECT_EQ(cell.second, it.GetValue());
176  it.Next();
177  }
178 
179  // Now 'values_' must not contain values.
180  for (const auto& pair : values_) {
181  const Eigen::Array3i cell_index(std::get<0>(pair.first),
182  std::get<1>(pair.first),
183  std::get<2>(pair.first));
184  ADD_FAILURE() << cell_index << " Probability: " << pair.second;
185  }
186 }
187 
188 TEST_F(RandomHybridGridTest, ToProto) {
189  const auto proto = ToProto(hybrid_grid_);
190  EXPECT_EQ(hybrid_grid_.resolution(), proto.resolution());
191 
192  ASSERT_EQ(proto.x_indices_size(), proto.y_indices_size());
193  ASSERT_EQ(proto.x_indices_size(), proto.z_indices_size());
194  ASSERT_EQ(proto.x_indices_size(), proto.values_size());
195 
196  ValueMap proto_map;
197  for (int i = 0; i < proto.x_indices_size(); ++i) {
198  proto_map[std::make_tuple(proto.x_indices(i), proto.y_indices(i),
199  proto.z_indices(i))] = proto.values(i);
200  }
201 
202  // Get hybrid_grid_ into the same format.
203  ValueMap hybrid_grid_map;
204  for (const auto i : hybrid_grid_) {
205  hybrid_grid_map[std::make_tuple(i.first.x(), i.first.y(), i.first.z())] =
206  i.second;
207  }
208 
209  EXPECT_EQ(proto_map, hybrid_grid_map);
210 }
211 
212 namespace {
213 
214 struct EigenComparator {
215  bool operator()(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) {
216  return std::forward_as_tuple(lhs.x(), lhs.y(), lhs.z()) <
217  std::forward_as_tuple(rhs.x(), rhs.y(), rhs.z());
218  }
219 };
220 
221 } // namespace
222 
223 TEST_F(RandomHybridGridTest, FromProto) {
224  const HybridGrid constructed_grid(ToProto(hybrid_grid_));
225 
226  std::map<Eigen::Vector3i, float, EigenComparator> member_map(
227  hybrid_grid_.begin(), hybrid_grid_.end());
228 
229  std::map<Eigen::Vector3i, float, EigenComparator> constructed_map(
230  constructed_grid.begin(), constructed_grid.end());
231 
232  EXPECT_EQ(member_map, constructed_map);
233 }
234 
235 } // namespace
236 } // namespace mapping_3d
237 } // namespace cartographer
hybrid_grid.h
cartographer::mapping::ComputeLookupTableToApplyOdds
std::vector< uint16 > ComputeLookupTableToApplyOdds(const float odds)
Definition: probability_values.cc:53
cartographer::mapping::kMinProbability
constexpr float kMinProbability
Definition: probability_values.h:38
hybrid_grid_
HybridGrid hybrid_grid_
Definition: hybrid_grid_test.cc:152
cartographer::mapping::ValueToProbability
float ValueToProbability(const uint16 value)
Definition: probability_values.h:65
cartographer::mapping::kMaxProbability
constexpr float kMaxProbability
Definition: probability_values.h:39
cartographer::mapping::Odds
float Odds(float probability)
Definition: probability_values.h:30
cartographer::mapping_3d::HybridGridBase< uint16 >::Iterator
typename Grid< uint16 >::Iterator Iterator
Definition: hybrid_grid.h:416
values_
ValueMap values_
Definition: hybrid_grid_test.cc:154
cartographer::sensor::FromProto
RangeData FromProto(const proto::RangeData &proto)
Definition: range_data.cc:33
cartographer::mapping_3d::ToProto
proto::HybridGrid ToProto(const HybridGrid &grid)
Definition: hybrid_grid.h:530

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