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

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