pose_graph_3d_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/3d/pose_graph_3d.h"
18 
19 #include "cartographer/mapping/internal/testing/test_helpers.h"
20 #include "cartographer/mapping/proto/serialization.pb.h"
21 #include "cartographer/transform/rigid_transform.h"
22 #include "cartographer/transform/rigid_transform_test_helpers.h"
23 #include "cartographer/transform/transform.h"
24 #include "gmock/gmock.h"
25 #include "google/protobuf/util/message_differencer.h"
26 
27 namespace cartographer {
28 namespace mapping {
29 namespace {
30 
31 using ::cartographer::mapping::optimization::OptimizationProblem3D;
32 using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions;
33 using ::cartographer::transform::Rigid3d;
34 
35 class MockOptimizationProblem3D : public OptimizationProblem3D {
36  public:
37  MockOptimizationProblem3D()
38  : OptimizationProblem3D(OptimizationProblemOptions{}) {}
39  ~MockOptimizationProblem3D() override = default;
40 
41  MOCK_METHOD3(Solve,
42  void(const std::vector<Constraint> &, const std::set<int> &,
43  const std::map<std::string, LandmarkNode> &));
44 };
45 
46 class PoseGraph3DForTesting : public PoseGraph3D {
47  public:
48  PoseGraph3DForTesting(
49  const proto::PoseGraphOptions &options,
50  std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
51  common::ThreadPool *thread_pool)
52  : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
53 
54  void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
55 };
56 
57 class PoseGraph3DTest : public ::testing::Test {
58  protected:
59  PoseGraph3DTest()
60  : thread_pool_(common::make_unique<common::ThreadPool>(1)) {}
61 
62  void SetUp() override {
63  const std::string kPoseGraphLua = R"text(
64  include "pose_graph.lua"
65  return POSE_GRAPH)text";
66  auto pose_graph_parameters = test::ResolveLuaParameters(kPoseGraphLua);
67  pose_graph_options_ = CreatePoseGraphOptions(pose_graph_parameters.get());
68  }
69 
70  void BuildPoseGraph() {
71  auto optimization_problem =
72  common::make_unique<optimization::OptimizationProblem3D>(
73  pose_graph_options_.optimization_problem_options());
74  pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
75  pose_graph_options_, std::move(optimization_problem),
76  thread_pool_.get());
77  }
78 
79  void BuildPoseGraphWithFakeOptimization() {
80  auto optimization_problem =
81  common::make_unique<MockOptimizationProblem3D>();
82  pose_graph_ = common::make_unique<PoseGraph3DForTesting>(
83  pose_graph_options_, std::move(optimization_problem),
84  thread_pool_.get());
85  }
86 
87  proto::PoseGraphOptions pose_graph_options_;
88  std::unique_ptr<common::ThreadPool> thread_pool_;
89  std::unique_ptr<PoseGraph3DForTesting> pose_graph_;
90 };
91 
92 TEST_F(PoseGraph3DTest, Empty) {
93  BuildPoseGraph();
94  pose_graph_->WaitForAllComputations();
95  EXPECT_TRUE(pose_graph_->GetAllSubmapData().empty());
96  EXPECT_TRUE(pose_graph_->constraints().empty());
97  EXPECT_TRUE(pose_graph_->GetConnectedTrajectories().empty());
98  EXPECT_TRUE(pose_graph_->GetAllSubmapPoses().empty());
99  EXPECT_TRUE(pose_graph_->GetTrajectoryNodes().empty());
100  EXPECT_TRUE(pose_graph_->GetTrajectoryNodePoses().empty());
101  EXPECT_TRUE(pose_graph_->GetTrajectoryData().empty());
102  proto::PoseGraph empty_proto;
103  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
104  pose_graph_->ToProto(), empty_proto));
105 }
106 
107 TEST_F(PoseGraph3DTest, BasicSerialization) {
108  BuildPoseGraph();
109  proto::PoseGraph proto;
110  auto fake_node = test::CreateFakeNode();
111  test::AddToProtoGraph(fake_node, &proto);
112  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node);
113  auto fake_submap = test::CreateFakeSubmap3D();
114  test::AddToProtoGraph(fake_submap, &proto);
115  pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap);
116  test::AddToProtoGraph(test::CreateFakeConstraint(fake_node, fake_submap),
117  &proto);
118  pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
119  test::AddToProtoGraph(
120  test::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto);
121  pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity());
122  pose_graph_->WaitForAllComputations();
123  proto::PoseGraph actual_proto = pose_graph_->ToProto();
124  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
125  proto.constraint(0), actual_proto.constraint(0)));
126  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
127  proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0)));
128  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
129  proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
130  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
131  proto.trajectory(0), actual_proto.trajectory(0)));
132  EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
133  proto.landmark_poses(0), actual_proto.landmark_poses(0)));
134  EXPECT_TRUE(
135  google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
136 }
137 
138 TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
139  BuildPoseGraphWithFakeOptimization();
140  const int trajectory_id = 2;
141  const int num_submaps_to_create = 5;
142  const int num_submaps_to_keep = 3;
143  const int num_nodes_per_submap = 2;
144  for (int i = 0; i < num_submaps_to_create; ++i) {
145  int submap_index = (i < 3) ? 42 + i : 100 + i;
146  auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index);
147  pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
148  for (int j = 0; j < num_nodes_per_submap; ++j) {
149  int node_index = 7 + num_nodes_per_submap * submap_index + j;
150  auto node = test::CreateFakeNode(trajectory_id, node_index);
151  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
152  proto::PoseGraph proto;
153  auto constraint = test::CreateFakeConstraint(node, submap);
154  // TODO(gaschler): Also remove inter constraints when all references are
155  // gone.
156  constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP);
157  test::AddToProtoGraph(constraint, &proto);
158  pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
159  }
160  }
161  pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
162  trajectory_id, num_submaps_to_keep));
163  pose_graph_->WaitForAllComputations();
164  EXPECT_EQ(
165  pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
166  num_submaps_to_create);
167  EXPECT_EQ(
168  pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
169  num_submaps_to_create);
170  EXPECT_EQ(
171  pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
172  num_nodes_per_submap * num_submaps_to_create);
173  EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
174  trajectory_id),
175  num_nodes_per_submap * num_submaps_to_create);
176  EXPECT_EQ(pose_graph_->constraints().size(),
177  num_nodes_per_submap * num_submaps_to_create);
178  for (int i = 0; i < 2; ++i) {
179  pose_graph_->RunFinalOptimization();
180  EXPECT_EQ(
181  pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
182  num_submaps_to_keep);
183  EXPECT_EQ(
184  pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
185  num_submaps_to_keep);
186  EXPECT_EQ(
187  pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
188  num_nodes_per_submap * num_submaps_to_keep);
189  EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
190  trajectory_id),
191  num_nodes_per_submap * num_submaps_to_keep);
192  EXPECT_EQ(pose_graph_->constraints().size(),
193  num_nodes_per_submap * num_submaps_to_keep);
194  }
195 }
196 
197 class EvenSubmapTrimmer : public PoseGraphTrimmer {
198  public:
199  explicit EvenSubmapTrimmer(int trajectory_id)
200  : trajectory_id_(trajectory_id) {}
201 
202  void Trim(Trimmable *pose_graph) override {
203  auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
204  for (const auto submap_id : submap_ids) {
205  if (submap_id.submap_index % 2 == 0) {
206  pose_graph->MarkSubmapAsTrimmed(submap_id);
207  }
208  }
209  }
210 
211  bool IsFinished() override { return false; }
212 
213  private:
214  int trajectory_id_;
215 };
216 
217 TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) {
218  BuildPoseGraphWithFakeOptimization();
219  const int trajectory_id = 2;
220  const int num_submaps_to_keep = 10;
221  const int num_submaps_to_create = 2 * num_submaps_to_keep;
222  const int num_nodes_per_submap = 3;
223  for (int i = 0; i < num_submaps_to_create; ++i) {
224  int submap_index = 42 + i;
225  auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index);
226  pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
227  for (int j = 0; j < num_nodes_per_submap; ++j) {
228  int node_index = 7 + num_nodes_per_submap * i + j;
229  auto node = test::CreateFakeNode(trajectory_id, node_index);
230  pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
231  proto::PoseGraph proto;
232  auto constraint = test::CreateFakeConstraint(node, submap);
233  constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP);
234  test::AddToProtoGraph(constraint, &proto);
235  pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
236  }
237  }
238  pose_graph_->AddTrimmer(
239  common::make_unique<EvenSubmapTrimmer>(trajectory_id));
240  pose_graph_->WaitForAllComputations();
241  EXPECT_EQ(
242  pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
243  num_submaps_to_create);
244  EXPECT_EQ(
245  pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
246  num_submaps_to_create);
247  EXPECT_EQ(
248  pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
249  num_nodes_per_submap * num_submaps_to_create);
250  EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
251  trajectory_id),
252  num_nodes_per_submap * num_submaps_to_create);
253  EXPECT_EQ(pose_graph_->constraints().size(),
254  num_nodes_per_submap * num_submaps_to_create);
255  for (int i = 0; i < 2; ++i) {
256  pose_graph_->RunFinalOptimization();
257  EXPECT_EQ(
258  pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
259  num_submaps_to_keep);
260  EXPECT_EQ(
261  pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
262  num_submaps_to_keep);
263  EXPECT_EQ(
264  pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
265  num_nodes_per_submap * num_submaps_to_keep);
266  EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
267  trajectory_id),
268  num_nodes_per_submap * num_submaps_to_keep);
269  EXPECT_EQ(pose_graph_->constraints().size(),
270  num_nodes_per_submap * num_submaps_to_keep);
271  }
272 }
273 
274 } // namespace
275 } // namespace mapping
276 } // namespace cartographer
cartographer::mapping::FromProto
PoseGraph::Constraint::Tag FromProto(const proto::PoseGraph::Constraint::Tag &proto)
Definition: pose_graph.cc:38
cartographer::mapping::PoseGraph3D::WaitForAllComputations
void WaitForAllComputations() EXCLUDES(mutex_)
Definition: pose_graph_3d.cc:403
rigid_transform_test_helpers.h
cartographer::mapping::test::CreateFakeConstraint
proto::PoseGraph::Constraint CreateFakeConstraint(const proto::Node &node, const proto::Submap &submap)
Definition: mapping/internal/testing/test_helpers.cc:91
cartographer::common::make_unique
_Unique_if< T >::_Single_object make_unique(Args &&... args)
Definition: make_unique.h:46
cartographer::mapping::test::AddToProtoGraph
void AddToProtoGraph(const proto::Node &node_data, proto::PoseGraph *pose_graph)
Definition: mapping/internal/testing/test_helpers.cc:131
cartographer::transform::Rigid3d
Rigid3< double > Rigid3d
Definition: rigid_transform.h:225
pose_graph_options_
proto::PoseGraphOptions pose_graph_options_
Definition: pose_graph_3d_test.cc:87
thread_pool_
std::unique_ptr< common::ThreadPool > thread_pool_
Definition: pose_graph_3d_test.cc:88
trajectory_id_
int trajectory_id_
Definition: pose_graph_3d_test.cc:214
test_helpers.h
cartographer::mapping::CreatePoseGraphOptions
proto::PoseGraphOptions CreatePoseGraphOptions(common::LuaParameterDictionary *const parameter_dictionary)
Definition: pose_graph.cc:72
pose_graph_3d.h
cartographer::mapping::test::CreateFakeSubmap3D
proto::Submap CreateFakeSubmap3D(int trajectory_id, int submap_index)
Definition: mapping/internal/testing/test_helpers.cc:71
pose_graph_
std::unique_ptr< PoseGraph3DForTesting > pose_graph_
Definition: pose_graph_3d_test.cc:89
cartographer::mapping::test::CreateFakeLandmark
proto::PoseGraph::LandmarkPose CreateFakeLandmark(const std::string &landmark_id, const transform::Rigid3d &global_pose)
Definition: mapping/internal/testing/test_helpers.cc:123
cartographer::mapping::test::CreateFakeNode
proto::Node CreateFakeNode(int trajectory_id, int node_index)
Definition: mapping/internal/testing/test_helpers.cc:81
rigid_transform.h
cartographer::mapping::test::ResolveLuaParameters
std::unique_ptr<::cartographer::common::LuaParameterDictionary > ResolveLuaParameters(const std::string &lua_code)
Definition: mapping/internal/testing/test_helpers.cc:31

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