overlapping_submaps_trimmer_2d_test.cc
Go to the documentation of this file.
1 /*
2  * Copyright 2018 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/internal/2d/overlapping_submaps_trimmer_2d.h"
18 
19 #include <vector>
20 
21 #include "cartographer/common/time.h"
22 #include "cartographer/mapping/2d/submap_2d.h"
23 #include "cartographer/mapping/id.h"
24 #include "cartographer/mapping/internal/testing/fake_trimmable.h"
25 #include "cartographer/transform/transform.h"
26 #include "gmock/gmock.h"
27 #include "gtest/gtest.h"
28 
29 namespace cartographer {
30 namespace mapping {
31 namespace {
32 
33 using ::cartographer::transform::Rigid2d;
34 using ::cartographer::transform::Rigid3d;
35 using ::testing::ElementsAre;
36 using ::testing::Field;
37 using ::testing::IsEmpty;
38 
39 class OverlappingSubmapsTrimmer2DTest : public ::testing::Test {
40  protected:
41  // Creates a submap with num_cells x num_cells grid.
42  void AddSquareSubmap(const Rigid2d& global_to_submap_frame,
43  const Rigid2d& local_to_submap_frame,
44  const Eigen::Vector2d& submap_corner, int submap_index,
45  int num_cells, bool is_finished) {
46  proto::Submap2D submap_2d;
47  submap_2d.set_num_range_data(1);
48  submap_2d.set_finished(is_finished);
49  *submap_2d.mutable_local_pose() =
50  transform::ToProto(transform::Embed3D(local_to_submap_frame));
51 
52  auto* grid = submap_2d.mutable_grid();
53  for (int i = 0; i < num_cells * num_cells; ++i) {
54  grid->add_cells(1);
55  }
56 
57  auto* map_limits = grid->mutable_limits();
58  map_limits->set_resolution(1.0);
59  *map_limits->mutable_max() = transform::ToProto(submap_corner);
60  map_limits->mutable_cell_limits()->set_num_x_cells(num_cells);
61  map_limits->mutable_cell_limits()->set_num_y_cells(num_cells);
62 
63  auto* know_cells_box = grid->mutable_known_cells_box();
64  know_cells_box->set_min_x(0);
65  know_cells_box->set_min_y(0);
66  know_cells_box->set_max_x(num_cells - 1);
67  know_cells_box->set_max_y(num_cells - 1);
68 
69  grid->mutable_probability_grid_2d();
70  fake_pose_graph_.mutable_submap_data()->Insert(
71  {0 /* trajectory_id */, submap_index},
72  {std::make_shared<const Submap2D>(submap_2d),
73  transform::Embed3D(global_to_submap_frame)});
74  }
75 
76  void AddTrajectoryNode(int node_index, int64 timestamp) {
77  TrajectoryNode::Data data;
78  data.time = common::FromUniversal(timestamp);
79 
80  fake_pose_graph_.mutable_trajectory_nodes()->Insert(
81  NodeId{0 /* trajectory_id */, node_index},
82  {std::make_shared<const TrajectoryNode::Data>(data), {} /* pose */});
83  }
84 
85  void AddConstraint(int submap_index, int node_index, bool is_intra_submap) {
86  fake_pose_graph_.mutable_constraints()->push_back(
87  {SubmapId{0 /* trajectory_id */, submap_index},
88  NodeId{0 /* trajectory_id */, node_index},
89  {} /* pose */,
90  is_intra_submap ? PoseGraphInterface::Constraint::INTRA_SUBMAP
91  : PoseGraphInterface::Constraint::INTER_SUBMAP});
92  }
93 
94  testing::FakeTrimmable fake_pose_graph_;
95 };
96 
97 ::testing::Matcher<const SubmapId&> EqualsSubmapId(const SubmapId& expected) {
98  return ::testing::AllOf(
99  Field(&SubmapId::trajectory_id, expected.trajectory_id),
100  Field(&SubmapId::submap_index, expected.submap_index));
101 }
102 
103 TEST_F(OverlappingSubmapsTrimmer2DTest, EmptyPoseGraph) {
104  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
105  0 /* min_covered_cells_count */,
106  0 /* min_added_submaps_count */);
107  trimmer.Trim(&fake_pose_graph_);
108  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
109 }
110 
111 TEST_F(OverlappingSubmapsTrimmer2DTest, TrimOneOfTwoOverlappingSubmaps) {
112  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
113  Rigid2d::Identity() /* local_from_submap_frame */,
114  Eigen::Vector2d(1., 1.) /* submap corner */,
115  0 /* submap_index */, 1 /* num_cells */,
116  true /* is_finished */);
117  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
118  Rigid2d::Identity() /* local_from_submap_frame */,
119  Eigen::Vector2d(1., 1.) /* submap corner */,
120  1 /* submap_index */, 1 /* num_cells */,
121  true /* is_finished */);
122  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
123  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
124  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
125  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
126 
127  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
128  0 /* min_covered_cells_count */,
129  0 /* min_added_submaps_count */);
130  trimmer.Trim(&fake_pose_graph_);
131  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
132  ElementsAre(EqualsSubmapId({0, 0})));
133 }
134 
135 TEST_F(OverlappingSubmapsTrimmer2DTest, TestMinAddedSubmapsCountParam) {
136  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
137  Rigid2d::Identity() /* local_from_submap_frame */,
138  Eigen::Vector2d(1., 1.) /* submap corner */,
139  0 /* submap_index */, 1 /* num_cells */,
140  true /* is_finished */);
141  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
142  Rigid2d::Identity() /* local_from_submap_frame */,
143  Eigen::Vector2d(1., 1.) /* submap corner */,
144  1 /* submap_index */, 1 /* num_cells */,
145  true /* is_finished */);
146  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
147  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
148  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
149  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
150 
151  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
152  0 /* min_covered_cells_count */,
153  2 /* min_added_submaps_count */);
154  trimmer.Trim(&fake_pose_graph_);
155  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
156 
157  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
158  Rigid2d::Identity() /* local_from_submap_frame */,
159  Eigen::Vector2d(1., 1.) /* submap corner */,
160  2 /* submap_index */, 1 /* num_cells */,
161  true /* is_finished */);
162  AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
163  AddConstraint(2 /*submap_index*/, 2 /*node_index*/, true);
164  trimmer.Trim(&fake_pose_graph_);
165  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
166  ElementsAre(EqualsSubmapId({0, 0}), EqualsSubmapId({0, 1})));
167 }
168 
169 TEST_F(OverlappingSubmapsTrimmer2DTest, DoNotTrimUnfinishedSubmap) {
170  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
171  Rigid2d::Identity() /* local_from_submap_frame */,
172  Eigen::Vector2d(1., 1.) /* submap corner */,
173  0 /* submap_index */, 1 /* num_cells */,
174  false /* is_finished */);
175  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
176  Rigid2d::Identity() /* local_from_submap_frame */,
177  Eigen::Vector2d(1., 1.) /* submap corner */,
178  1 /* submap_index */, 1 /* num_cells */,
179  true /* is_finished */);
180  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
181  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
182  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
183  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
184 
185  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
186  0 /* min_covered_cells_count */,
187  0 /* min_added_submaps_count */);
188  trimmer.Trim(&fake_pose_graph_);
189  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(), IsEmpty());
190 }
191 
192 TEST_F(OverlappingSubmapsTrimmer2DTest, UseOnlyIntraSubmapsToComputeFreshness) {
193  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
194  Rigid2d::Identity() /* local_from_submap_frame */,
195  Eigen::Vector2d(1., 1.) /* submap corner */,
196  0 /* submap_index */, 1 /* num_cells */,
197  true /* is_finished */);
198  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
199  Rigid2d::Identity() /* local_from_submap_frame */,
200  Eigen::Vector2d(1., 1.) /* submap corner */,
201  1 /* submap_index */, 1 /* num_cells */,
202  true /* is_finished */);
203  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
204  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
205  AddTrajectoryNode(2 /* node_index */, 3000 /* timestamp */);
206  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, false);
207  AddConstraint(0 /*submap_index*/, 2 /*node_index*/, true);
208  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
209 
210  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
211  0 /* min_covered_cells_count */,
212  0 /* min_added_submaps_count */);
213  trimmer.Trim(&fake_pose_graph_);
214  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
215  ElementsAre(EqualsSubmapId({0, 1})));
216 }
217 
218 // This test covers two 4-cells submaps that overlap each other displaced like:
219 // ___
220 // _| |
221 // | |_ _|
222 // |___|
223 //
224 // The background submap should be trimmed, since it has only 3 cells
225 // not-covered by another submap.
226 TEST_F(OverlappingSubmapsTrimmer2DTest, TrimSubmapWithLowCoveredCellsCount) {
227  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
228  Rigid2d::Identity() /* local_from_submap_frame */,
229  Eigen::Vector2d(1., 1.) /* submap corner */,
230  0 /* submap_index */, 2 /* num_cells */,
231  true /* is_finished */);
232  AddSquareSubmap(Rigid2d::Translation(
233  Eigen::Vector2d(1., 1.)) /* global_from_submap_frame */,
234  Rigid2d::Identity() /* local_from_submap_frame */,
235  Eigen::Vector2d(1., 1.) /* submap corner */,
236  1 /* submap_index */, 2 /* num_cells */,
237  true /* is_finished */);
238  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
239  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
240  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
241  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
242 
243  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
244  4 /* min_covered_cells_count */,
245  0 /* min_added_submaps_count */);
246  trimmer.Trim(&fake_pose_graph_);
247  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
248  ElementsAre(EqualsSubmapId({0, 0})));
249 }
250 
251 TEST_F(OverlappingSubmapsTrimmer2DTest, TestTransformations) {
252  AddSquareSubmap(Rigid2d::Identity() /* global_from_submap_frame */,
253  Rigid2d::Identity() /* local_from_submap_frame */,
254  Eigen::Vector2d(1., 1.) /* submap corner */,
255  0 /* submap_index */, 1 /* num_cells */,
256  true /* is_finished */);
257  const Rigid2d transform(Eigen::Vector2d(1., 1.), M_PI / 2);
258  AddSquareSubmap(transform /* global_from_submap_frame */,
259  transform /* local_from_submap_frame */,
260  Eigen::Vector2d(1., 1.) /* submap corner */,
261  1 /* submap_index */, 1 /* num_cells */,
262  true /* is_finished */);
263  AddTrajectoryNode(0 /* node_index */, 1000 /* timestamp */);
264  AddTrajectoryNode(1 /* node_index */, 2000 /* timestamp */);
265  AddConstraint(0 /*submap_index*/, 0 /*node_index*/, true);
266  AddConstraint(1 /*submap_index*/, 1 /*node_index*/, true);
267 
268  OverlappingSubmapsTrimmer2D trimmer(1 /* fresh_submaps_count */,
269  0 /* min_covered_cells_count */,
270  0 /* min_added_submaps_count */);
271  trimmer.Trim(&fake_pose_graph_);
272  EXPECT_THAT(fake_pose_graph_.trimmed_submaps(),
273  ElementsAre(EqualsSubmapId({0, 0})));
274 }
275 
276 } // namespace
277 } // namespace mapping
278 } // namespace cartographer
fake_pose_graph_
testing::FakeTrimmable fake_pose_graph_
Definition: overlapping_submaps_trimmer_2d_test.cc:94
cartographer::transform::Rigid3d
Rigid3< double > Rigid3d
Definition: rigid_transform.h:225
cartographer::transform::Embed3D
Rigid3< T > Embed3D(const Rigid2< T > &transform)
Definition: transform.h:110
cartographer::int64
int64_t int64
Definition: port.h:33
cartographer::transform::Rigid2d
Rigid2< double > Rigid2d
Definition: rigid_transform.h:119
cartographer::common::FromUniversal
Time FromUniversal(const int64 ticks)
Definition: time.cc:34
cartographer::transform::ToProto
proto::Rigid2d ToProto(const transform::Rigid2d &transform)
Definition: transform.cc:48
fake_trimmable.h
overlapping_submaps_trimmer_2d.h
submap_2d.h

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