From 7a7908ebb9c2c06cfae40c79e7f1cb91c02ff0b8 Mon Sep 17 00:00:00 2001 From: gaschler Date: Mon, 19 Mar 2018 11:43:19 +0100 Subject: [PATCH] Test PureLocalizationTrimmer with actual pose graph (#1004) --- .../mapping/internal/3d/pose_graph_3d_test.cc | 104 ++++++++++++++++-- cartographer/mapping/pose_graph_trimmer.cc | 2 +- 2 files changed, 98 insertions(+), 8 deletions(-) diff --git a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc index 560d9a3..627942b 100644 --- a/cartographer/mapping/internal/3d/pose_graph_3d_test.cc +++ b/cartographer/mapping/internal/3d/pose_graph_3d_test.cc @@ -26,16 +26,30 @@ namespace cartographer { namespace mapping { +namespace pose_graph { + +class FakeOptimizationProblem3D : public OptimizationProblem3D { + public: + FakeOptimizationProblem3D() + : OptimizationProblem3D(proto::OptimizationProblemOptions{}) {} + ~FakeOptimizationProblem3D() override = default; + void Solve( + const std::vector& constraints, + const std::set& frozen_trajectories, + const std::map& landmark_nodes) override {} +}; + +} // namespace pose_graph + namespace { class PoseGraph3DForTesting : public PoseGraph3D { public: - PoseGraph3DForTesting(const proto::PoseGraphOptions& options, - common::ThreadPool* thread_pool) - : PoseGraph3D(options, - common::make_unique( - options.optimization_problem_options()), - thread_pool) {} + PoseGraph3DForTesting( + const proto::PoseGraphOptions& options, + std::unique_ptr optimization_problem, + common::ThreadPool* thread_pool) + : PoseGraph3D(options, std::move(optimization_problem), thread_pool) {} void WaitForAllComputations() { PoseGraph3D::WaitForAllComputations(); } }; @@ -51,8 +65,23 @@ class PoseGraph3DTest : public ::testing::Test { return POSE_GRAPH)text"; auto pose_graph_parameters = test::ResolveLuaParameters(kPoseGraphLua); pose_graph_options_ = CreatePoseGraphOptions(pose_graph_parameters.get()); + } + + void BuildPoseGraph() { + auto optimization_problem = + common::make_unique( + pose_graph_options_.optimization_problem_options()); pose_graph_ = common::make_unique( - pose_graph_options_, thread_pool_.get()); + pose_graph_options_, std::move(optimization_problem), + thread_pool_.get()); + } + + void BuildPoseGraphWithFakeOptimization() { + auto optimization_problem = + common::make_unique(); + pose_graph_ = common::make_unique( + pose_graph_options_, std::move(optimization_problem), + thread_pool_.get()); } proto::PoseGraphOptions pose_graph_options_; @@ -61,6 +90,7 @@ class PoseGraph3DTest : public ::testing::Test { }; TEST_F(PoseGraph3DTest, Empty) { + BuildPoseGraph(); pose_graph_->WaitForAllComputations(); EXPECT_TRUE(pose_graph_->GetAllSubmapData().empty()); EXPECT_TRUE(pose_graph_->constraints().empty()); @@ -75,6 +105,7 @@ TEST_F(PoseGraph3DTest, Empty) { } TEST_F(PoseGraph3DTest, BasicSerialization) { + BuildPoseGraph(); proto::PoseGraph proto; auto fake_node = test::CreateFakeNode(); test::AddToProtoGraph(fake_node, &proto); @@ -100,6 +131,65 @@ TEST_F(PoseGraph3DTest, BasicSerialization) { google::protobuf::util::MessageDifferencer::Equals(proto, actual_proto)); } +TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) { + BuildPoseGraphWithFakeOptimization(); + const int trajectory_id = 2; + const int num_submaps_to_create = 5; + const int num_submaps_to_keep = 3; + const int num_nodes_per_submap = 2; + for (int i = 0; i < num_submaps_to_create; ++i) { + int submap_index = 42 + i; + auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index); + pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap); + for (int j = 0; j < num_nodes_per_submap; ++j) { + int node_index = 7 + num_nodes_per_submap * i + j; + auto node = test::CreateFakeNode(trajectory_id, node_index); + pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node); + proto::PoseGraph proto; + auto constraint = test::CreateFakeConstraint(node, submap); + // TODO(gaschler): Also remove inter constraints when all references are + // gone. + constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP); + test::AddToProtoGraph(constraint, &proto); + pose_graph_->AddSerializedConstraints(FromProto(proto.constraint())); + } + } + pose_graph_->AddTrimmer(common::make_unique( + trajectory_id, num_submaps_to_keep)); + pose_graph_->WaitForAllComputations(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_create); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_create); + EXPECT_EQ(pose_graph_->constraints().size(), + num_nodes_per_submap * num_submaps_to_create); + for (int i = 0; i < 2; ++i) { + pose_graph_->RunFinalOptimization(); + EXPECT_EQ( + pose_graph_->GetAllSubmapPoses().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetAllSubmapData().SizeOfTrajectoryOrZero(trajectory_id), + num_submaps_to_keep); + EXPECT_EQ( + pose_graph_->GetTrajectoryNodes().SizeOfTrajectoryOrZero(trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->GetTrajectoryNodePoses().SizeOfTrajectoryOrZero( + trajectory_id), + num_nodes_per_submap * num_submaps_to_keep); + EXPECT_EQ(pose_graph_->constraints().size(), + num_nodes_per_submap * num_submaps_to_keep); + } +} + } // namespace } // namespace mapping } // namespace cartographer diff --git a/cartographer/mapping/pose_graph_trimmer.cc b/cartographer/mapping/pose_graph_trimmer.cc index a7c7384..530ad62 100644 --- a/cartographer/mapping/pose_graph_trimmer.cc +++ b/cartographer/mapping/pose_graph_trimmer.cc @@ -33,7 +33,7 @@ void PureLocalizationTrimmer::Trim(Trimmable* const pose_graph) { } auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_); - for (int i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) { + for (std::size_t i = 0; i + num_submaps_to_keep_ < submap_ids.size(); ++i) { pose_graph->MarkSubmapAsTrimmed(submap_ids.at(i)); }