pose_graph_3d_test.cc
Go to the documentation of this file.
00001 /*
00002  * Copyright 2018 The Cartographer Authors
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *      http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "cartographer/mapping/internal/3d/pose_graph_3d.h"
00018 
00019 #include "cartographer/mapping/internal/testing/test_helpers.h"
00020 #include "cartographer/mapping/proto/serialization.pb.h"
00021 #include "cartographer/transform/rigid_transform.h"
00022 #include "cartographer/transform/rigid_transform_test_helpers.h"
00023 #include "cartographer/transform/transform.h"
00024 #include "gmock/gmock.h"
00025 #include "google/protobuf/util/message_differencer.h"
00026 
00027 namespace cartographer {
00028 namespace mapping {
00029 namespace {
00030 
00031 using ::cartographer::mapping::optimization::OptimizationProblem3D;
00032 using ::cartographer::mapping::optimization::proto::OptimizationProblemOptions;
00033 using ::cartographer::transform::Rigid3d;
00034 
00035 class MockOptimizationProblem3D : public OptimizationProblem3D {
00036  public:
00037   MockOptimizationProblem3D()
00038       : OptimizationProblem3D(OptimizationProblemOptions{}) {}
00039   ~MockOptimizationProblem3D() override = default;
00040 
00041   MOCK_METHOD3(Solve,
00042                void(const std::vector<Constraint> &,
00043                     const std::map<int, PoseGraphInterface::TrajectoryState> &,
00044                     const std::map<std::string, LandmarkNode> &));
00045 };
00046 
00047 class PoseGraph3DForTesting : public PoseGraph3D {
00048  public:
00049   PoseGraph3DForTesting(
00050       const proto::PoseGraphOptions &options,
00051       std::unique_ptr<optimization::OptimizationProblem3D> optimization_problem,
00052       common::ThreadPool *thread_pool)
00053       : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {}
00054 
00055   void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); }
00056 };
00057 
00058 class PoseGraph3DTest : public ::testing::Test {
00059  protected:
00060   PoseGraph3DTest() : thread_pool_(absl::make_unique<common::ThreadPool>(1)) {}
00061 
00062   void SetUp() override {
00063     const std::string kPoseGraphLua = R"text(
00064       include "pose_graph.lua"
00065       return POSE_GRAPH)text";
00066     auto pose_graph_parameters = testing::ResolveLuaParameters(kPoseGraphLua);
00067     pose_graph_options_ = CreatePoseGraphOptions(pose_graph_parameters.get());
00068   }
00069 
00070   void BuildPoseGraph() {
00071     auto optimization_problem =
00072         absl::make_unique<optimization::OptimizationProblem3D>(
00073             pose_graph_options_.optimization_problem_options());
00074     pose_graph_ = absl::make_unique<PoseGraph3DForTesting>(
00075         pose_graph_options_, std::move(optimization_problem),
00076         thread_pool_.get());
00077   }
00078 
00079   void BuildPoseGraphWithFakeOptimization() {
00080     auto optimization_problem = absl::make_unique<MockOptimizationProblem3D>();
00081     pose_graph_ = absl::make_unique<PoseGraph3DForTesting>(
00082         pose_graph_options_, std::move(optimization_problem),
00083         thread_pool_.get());
00084   }
00085 
00086   proto::PoseGraphOptions pose_graph_options_;
00087   std::unique_ptr<common::ThreadPool> thread_pool_;
00088   std::unique_ptr<PoseGraph3DForTesting> pose_graph_;
00089 };
00090 
00091 TEST_F(PoseGraph3DTest, Empty) {
00092   BuildPoseGraph();
00093   pose_graph_->WaitForAllComputations();
00094   EXPECT_TRUE(pose_graph_->GetAllSubmapData().empty());
00095   EXPECT_TRUE(pose_graph_->constraints().empty());
00096   EXPECT_TRUE(pose_graph_->GetConnectedTrajectories().empty());
00097   EXPECT_TRUE(pose_graph_->GetAllSubmapPoses().empty());
00098   EXPECT_TRUE(pose_graph_->GetTrajectoryNodes().empty());
00099   EXPECT_TRUE(pose_graph_->GetTrajectoryNodePoses().empty());
00100   EXPECT_TRUE(pose_graph_->GetTrajectoryData().empty());
00101   proto::PoseGraph empty_proto;
00102   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00103       pose_graph_->ToProto(/*include_unfinished_submaps=*/true), empty_proto));
00104 }
00105 
00106 TEST_F(PoseGraph3DTest, BasicSerialization) {
00107   BuildPoseGraph();
00108   proto::PoseGraph proto;
00109   auto fake_node = testing::CreateFakeNode();
00110   testing::AddToProtoGraph(fake_node, &proto);
00111   pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node);
00112   auto fake_submap = testing::CreateFakeSubmap3D();
00113   testing::AddToProtoGraph(fake_submap, &proto);
00114   pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap);
00115   testing::AddToProtoGraph(
00116       testing::CreateFakeConstraint(fake_node, fake_submap), &proto);
00117   pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
00118   testing::AddToProtoGraph(
00119       testing::CreateFakeLandmark("landmark_id", Rigid3d::Identity()), &proto);
00120   pose_graph_->SetLandmarkPose("landmark_id", Rigid3d::Identity());
00121   pose_graph_->WaitForAllComputations();
00122   proto::PoseGraph actual_proto =
00123       pose_graph_->ToProto(/*include_unfinished_submaps=*/true);
00124   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00125       proto.constraint(0), actual_proto.constraint(0)));
00126   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00127       proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0)));
00128   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00129       proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
00130   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00131       proto.trajectory(0), actual_proto.trajectory(0)));
00132   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00133       proto.landmark_poses(0), actual_proto.landmark_poses(0)));
00134   EXPECT_TRUE(
00135       google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto));
00136 }
00137 
00138 TEST_F(PoseGraph3DTest, SerializationWithUnfinishedSubmaps) {
00139   BuildPoseGraph();
00140   proto::PoseGraph proto;
00141 
00142   // Create three nodes.
00143   auto fake_node_1 = testing::CreateFakeNode(1, 1);
00144   testing::AddToProtoGraph(fake_node_1, &proto);
00145   pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_1);
00146   auto fake_node_2 = testing::CreateFakeNode(1, 2);
00147   testing::AddToProtoGraph(fake_node_2, &proto);
00148   pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_2);
00149   auto fake_node_3 = testing::CreateFakeNode(1, 3);
00150   testing::AddToProtoGraph(fake_node_3, &proto);
00151   pose_graph_->AddNodeFromProto(Rigid3d::Identity(), fake_node_3);
00152 
00153   // Create two submaps: one finished, the other not.
00154   auto fake_submap_1 = testing::CreateFakeSubmap3D(1, 1, true);
00155   testing::AddToProtoGraph(fake_submap_1, &proto);
00156   pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_1);
00157   auto fake_submap_2 = testing::CreateFakeSubmap3D(1, 2, false);
00158   testing::AddToProtoGraph(fake_submap_2, &proto);
00159   pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), fake_submap_2);
00160 
00161   // Connect node 1 to submap 1, node 2 to submaps 1 and 2, node 3 to submap 2.
00162   testing::AddToProtoGraph(
00163       testing::CreateFakeConstraint(fake_node_1, fake_submap_1), &proto);
00164   testing::AddToProtoGraph(
00165       testing::CreateFakeConstraint(fake_node_2, fake_submap_1), &proto);
00166   testing::AddToProtoGraph(
00167       testing::CreateFakeConstraint(fake_node_2, fake_submap_2), &proto);
00168   testing::AddToProtoGraph(
00169       testing::CreateFakeConstraint(fake_node_3, fake_submap_2), &proto);
00170   pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
00171 
00172   pose_graph_->WaitForAllComputations();
00173   proto::PoseGraph actual_proto =
00174       pose_graph_->ToProto(/*include_unfinished_submaps=*/false);
00175   EXPECT_EQ(actual_proto.constraint_size(), 2);
00176   EXPECT_EQ(actual_proto.trajectory_size(), 1);
00177   EXPECT_EQ(actual_proto.trajectory(0).node_size(), 3);
00178   EXPECT_EQ(actual_proto.trajectory(0).submap_size(), 1);
00179   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00180       proto.constraint(0), actual_proto.constraint(0)));
00181   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00182       proto.constraint(1), actual_proto.constraint(1)));
00183   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00184       proto.trajectory(0).node(0), actual_proto.trajectory(0).node(0)));
00185   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00186       proto.trajectory(0).node(1), actual_proto.trajectory(0).node(1)));
00187   EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(
00188       proto.trajectory(0).submap(0), actual_proto.trajectory(0).submap(0)));
00189 }
00190 
00191 TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
00192   BuildPoseGraphWithFakeOptimization();
00193   const int trajectory_id = 2;
00194   const int num_submaps_to_create = 5;
00195   const int num_submaps_to_keep = 3;
00196   const int num_nodes_per_submap = 2;
00197   for (int i = 0; i < num_submaps_to_create; ++i) {
00198     int submap_index = (i < 3) ? 42 + i : 100 + i;
00199     auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index);
00200     pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
00201     for (int j = 0; j < num_nodes_per_submap; ++j) {
00202       int node_index = 7 + num_nodes_per_submap * submap_index + j;
00203       auto node = testing::CreateFakeNode(trajectory_id, node_index);
00204       pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
00205       proto::PoseGraph proto;
00206       auto constraint = testing::CreateFakeConstraint(node, submap);
00207       // TODO(gaschler): Also remove inter constraints when all references are
00208       // gone.
00209       constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP);
00210       testing::AddToProtoGraph(constraint, &proto);
00211       pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
00212     }
00213   }
00214   pose_graph_->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
00215       trajectory_id, num_submaps_to_keep));
00216   pose_graph_->WaitForAllComputations();
00217   EXPECT_EQ(
00218       pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
00219       num_submaps_to_create);
00220   EXPECT_EQ(
00221       pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
00222       num_submaps_to_create);
00223   EXPECT_EQ(
00224       pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
00225       num_nodes_per_submap * num_submaps_to_create);
00226   EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
00227                 trajectory_id),
00228             num_nodes_per_submap * num_submaps_to_create);
00229   EXPECT_EQ(pose_graph_->constraints().size(),
00230             num_nodes_per_submap * num_submaps_to_create);
00231   for (int i = 0; i < 2; ++i) {
00232     pose_graph_->RunFinalOptimization();
00233     EXPECT_EQ(
00234         pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
00235         num_submaps_to_keep);
00236     EXPECT_EQ(
00237         pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
00238         num_submaps_to_keep);
00239     EXPECT_EQ(
00240         pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
00241         num_nodes_per_submap * num_submaps_to_keep);
00242     EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
00243                   trajectory_id),
00244               num_nodes_per_submap * num_submaps_to_keep);
00245     EXPECT_EQ(pose_graph_->constraints().size(),
00246               num_nodes_per_submap * num_submaps_to_keep);
00247   }
00248 }
00249 
00250 class EvenSubmapTrimmer : public PoseGraphTrimmer {
00251  public:
00252   explicit EvenSubmapTrimmer(int trajectory_id)
00253       : trajectory_id_(trajectory_id) {}
00254 
00255   void Trim(Trimmable *pose_graph) override {
00256     auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
00257     for (const auto submap_id : submap_ids) {
00258       if (submap_id.submap_index % 2 == 0) {
00259         pose_graph->TrimSubmap(submap_id);
00260       }
00261     }
00262   }
00263 
00264   bool IsFinished() override { return false; }
00265 
00266  private:
00267   int trajectory_id_;
00268 };
00269 
00270 TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) {
00271   BuildPoseGraphWithFakeOptimization();
00272   const int trajectory_id = 2;
00273   const int num_submaps_to_keep = 10;
00274   const int num_submaps_to_create = 2 * num_submaps_to_keep;
00275   const int num_nodes_per_submap = 3;
00276   for (int i = 0; i < num_submaps_to_create; ++i) {
00277     int submap_index = 42 + i;
00278     auto submap = testing::CreateFakeSubmap3D(trajectory_id, submap_index);
00279     pose_graph_->AddSubmapFromProto(Rigid3d::Identity(), submap);
00280     for (int j = 0; j < num_nodes_per_submap; ++j) {
00281       int node_index = 7 + num_nodes_per_submap * i + j;
00282       auto node = testing::CreateFakeNode(trajectory_id, node_index);
00283       pose_graph_->AddNodeFromProto(Rigid3d::Identity(), node);
00284       proto::PoseGraph proto;
00285       auto constraint = testing::CreateFakeConstraint(node, submap);
00286       constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP);
00287       testing::AddToProtoGraph(constraint, &proto);
00288       pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
00289     }
00290   }
00291   pose_graph_->AddTrimmer(absl::make_unique<EvenSubmapTrimmer>(trajectory_id));
00292   pose_graph_->WaitForAllComputations();
00293   EXPECT_EQ(
00294       pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
00295       num_submaps_to_create);
00296   EXPECT_EQ(
00297       pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
00298       num_submaps_to_create);
00299   EXPECT_EQ(
00300       pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
00301       num_nodes_per_submap * num_submaps_to_create);
00302   EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
00303                 trajectory_id),
00304             num_nodes_per_submap * num_submaps_to_create);
00305   EXPECT_EQ(pose_graph_->constraints().size(),
00306             num_nodes_per_submap * num_submaps_to_create);
00307   for (int i = 0; i < 2; ++i) {
00308     pose_graph_->RunFinalOptimization();
00309     EXPECT_EQ(
00310         pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id),
00311         num_submaps_to_keep);
00312     EXPECT_EQ(
00313         pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id),
00314         num_submaps_to_keep);
00315     EXPECT_EQ(
00316         pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id),
00317         num_nodes_per_submap * num_submaps_to_keep);
00318     EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero(
00319                   trajectory_id),
00320               num_nodes_per_submap * num_submaps_to_keep);
00321     EXPECT_EQ(pose_graph_->constraints().size(),
00322               num_nodes_per_submap * num_submaps_to_keep);
00323   }
00324 }
00325 
00326 }  // namespace
00327 }  // namespace mapping
00328 }  // namespace cartographer


cartographer
Author(s): The Cartographer Authors
autogenerated on Thu May 9 2019 02:27:35