probability_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/2d/probability_grid.h"
18 
19 #include <random>
20 
21 #include "cartographer/mapping/probability_values.h"
22 #include "gtest/gtest.h"
23 
24 namespace cartographer {
25 namespace mapping {
26 namespace {
27 
28 using Eigen::Array2i;
29 using Eigen::Vector2f;
30 
31 TEST(ProbabilityGridTest, ProtoConstructor) {
32  proto::Grid2D proto;
33  const MapLimits limits(1., {2., 3.}, CellLimits(4., 5.));
34  *proto.mutable_limits() = ToProto(limits);
35  for (int i = 6; i < 12; ++i) {
36  proto.mutable_cells()->Add(static_cast<uint16>(i));
37  }
38  proto.mutable_known_cells_box()->set_max_x(19);
39  proto.mutable_known_cells_box()->set_max_y(20);
40  proto.mutable_known_cells_box()->set_min_x(21);
41  proto.mutable_known_cells_box()->set_min_y(22);
42  proto.mutable_probability_grid_2d();
43 
44  ProbabilityGrid grid(proto);
45  EXPECT_EQ(proto.limits().DebugString(), ToProto(grid.limits()).DebugString());
46 
47  // TODO(macmason): Figure out how to test the contents of cells_ and
48  // {min, max}_{x, y}_ gracefully.
49 }
50 
51 TEST(ProbabilityGridTest, ToProto) {
52  ProbabilityGrid probability_grid(
53  MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
54 
55  const auto proto = probability_grid.ToProto();
56  EXPECT_EQ(ToProto(probability_grid.limits()).DebugString(),
57  proto.limits().DebugString());
58 
59  // TODO(macmason): Figure out how to test the contents of cells_ and
60  // {min, max}_{x, y}_ gracefully.
61 }
62 
63 TEST(ProbabilityGridTest, ApplyOdds) {
64  ProbabilityGrid probability_grid(
65  MapLimits(1., Eigen::Vector2d(1., 1.), CellLimits(2, 2)));
66  const MapLimits& limits = probability_grid.limits();
67 
68  EXPECT_TRUE(limits.Contains(Array2i(0, 0)));
69  EXPECT_TRUE(limits.Contains(Array2i(0, 1)));
70  EXPECT_TRUE(limits.Contains(Array2i(1, 0)));
71  EXPECT_TRUE(limits.Contains(Array2i(1, 1)));
72  EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 0)));
73  EXPECT_FALSE(probability_grid.IsKnown(Array2i(0, 1)));
74  EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 0)));
75  EXPECT_FALSE(probability_grid.IsKnown(Array2i(1, 1)));
76 
77  probability_grid.SetProbability(Array2i(1, 0), 0.5);
78 
79  probability_grid.ApplyLookupTable(
80  Array2i(1, 0),
81  ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
82  probability_grid.FinishUpdate();
83  EXPECT_GT(probability_grid.GetProbability(Array2i(1, 0)), 0.5);
84 
85  probability_grid.SetProbability(Array2i(0, 1), 0.5);
86  probability_grid.ApplyLookupTable(
87  Array2i(0, 1),
88  ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.1)));
89  probability_grid.FinishUpdate();
90  EXPECT_LT(probability_grid.GetProbability(Array2i(0, 1)), 0.5);
91 
92  // Tests adding odds to an unknown cell.
93  probability_grid.ApplyLookupTable(
94  Array2i(1, 1),
95  ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.42)));
96  EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
97 
98  // Tests that further updates are ignored if FinishUpdate() isn't called.
99  probability_grid.ApplyLookupTable(
100  Array2i(1, 1),
101  ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
102  EXPECT_NEAR(probability_grid.GetProbability(Array2i(1, 1)), 0.42, 1e-4);
103  probability_grid.FinishUpdate();
104  probability_grid.ApplyLookupTable(
105  Array2i(1, 1),
106  ComputeLookupTableToApplyCorrespondenceCostOdds(Odds(0.9)));
107  EXPECT_GT(probability_grid.GetProbability(Array2i(1, 1)), 0.42);
108 }
109 
110 TEST(ProbabilityGridTest, GetProbability) {
111  ProbabilityGrid probability_grid(
112  MapLimits(1., Eigen::Vector2d(1., 2.), CellLimits(2, 2)));
113 
114  const MapLimits& limits = probability_grid.limits();
115  EXPECT_EQ(1., limits.max().x());
116  EXPECT_EQ(2., limits.max().y());
117 
118  const CellLimits& cell_limits = limits.cell_limits();
119  ASSERT_EQ(2, cell_limits.num_x_cells);
120  ASSERT_EQ(2, cell_limits.num_y_cells);
121 
122  probability_grid.SetProbability(limits.GetCellIndex(Vector2f(-0.5f, 0.5f)),
123  kMaxProbability);
124  EXPECT_NEAR(probability_grid.GetProbability(
125  limits.GetCellIndex(Vector2f(-0.5f, 0.5f))),
126  kMaxProbability, 1e-6);
127  for (const Array2i& xy_index : {limits.GetCellIndex(Vector2f(-0.5f, 1.5f)),
128  limits.GetCellIndex(Vector2f(0.5f, 0.5f)),
129  limits.GetCellIndex(Vector2f(0.5f, 1.5f))}) {
130  EXPECT_TRUE(limits.Contains(xy_index));
131  EXPECT_FALSE(probability_grid.IsKnown(xy_index));
132  }
133 }
134 
135 TEST(ProbabilityGridTest, GetCellIndex) {
136  ProbabilityGrid probability_grid(
137  MapLimits(2., Eigen::Vector2d(8., 14.), CellLimits(14, 8)));
138 
139  const MapLimits& limits = probability_grid.limits();
140  const CellLimits& cell_limits = limits.cell_limits();
141  ASSERT_EQ(14, cell_limits.num_x_cells);
142  ASSERT_EQ(8, cell_limits.num_y_cells);
143  EXPECT_TRUE(
144  (Array2i(0, 0) == limits.GetCellIndex(Vector2f(7.f, 13.f))).all());
145  EXPECT_TRUE(
146  (Array2i(13, 0) == limits.GetCellIndex(Vector2f(7.f, -13.f))).all());
147  EXPECT_TRUE(
148  (Array2i(0, 7) == limits.GetCellIndex(Vector2f(-7.f, 13.f))).all());
149  EXPECT_TRUE(
150  (Array2i(13, 7) == limits.GetCellIndex(Vector2f(-7.f, -13.f))).all());
151 
152  // Check around the origin.
153  EXPECT_TRUE(
154  (Array2i(6, 3) == limits.GetCellIndex(Vector2f(0.5f, 0.5f))).all());
155  EXPECT_TRUE(
156  (Array2i(6, 3) == limits.GetCellIndex(Vector2f(1.5f, 1.5f))).all());
157  EXPECT_TRUE(
158  (Array2i(7, 3) == limits.GetCellIndex(Vector2f(0.5f, -0.5f))).all());
159  EXPECT_TRUE(
160  (Array2i(6, 4) == limits.GetCellIndex(Vector2f(-0.5f, 0.5f))).all());
161  EXPECT_TRUE(
162  (Array2i(7, 4) == limits.GetCellIndex(Vector2f(-0.5f, -0.5f))).all());
163 }
164 
165 TEST(ProbabilityGridTest, CorrectCropping) {
166  // Create a probability grid with random values.
167  std::mt19937 rng(42);
168  std::uniform_real_distribution<float> value_distribution(kMinProbability,
169  kMaxProbability);
170  ProbabilityGrid probability_grid(
171  MapLimits(0.05, Eigen::Vector2d(10., 10.), CellLimits(400, 400)));
172  for (const Array2i& xy_index :
173  XYIndexRangeIterator(Array2i(100, 100), Array2i(299, 299))) {
174  probability_grid.SetProbability(xy_index, value_distribution(rng));
175  }
176  Array2i offset;
177  CellLimits limits;
178  probability_grid.ComputeCroppedLimits(&offset, &limits);
179  EXPECT_TRUE((offset == Array2i(100, 100)).all());
180  EXPECT_EQ(limits.num_x_cells, 200);
181  EXPECT_EQ(limits.num_y_cells, 200);
182 }
183 
184 } // namespace
185 } // namespace mapping
186 } // namespace cartographer
cartographer::mapping::ComputeLookupTableToApplyCorrespondenceCostOdds
std::vector< uint16 > ComputeLookupTableToApplyCorrespondenceCostOdds(float odds)
Definition: probability_values.cc:85
cartographer::mapping::kMinProbability
constexpr float kMinProbability
Definition: probability_values.h:64
cartographer::mapping::kMaxProbability
constexpr float kMaxProbability
Definition: probability_values.h:65
cartographer::mapping::Odds
float Odds(float probability)
Definition: probability_values.h:48
probability_values.h
probability_grid.h
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