12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Test trimming within trajectory (#1020)

master
gaschler 2018-03-27 12:50:35 +02:00 committed by GitHub
parent e0e1c081e7
commit 94f564d871
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 79 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
cartographer/mapping/internal/3d/pose_graph_3d_test.cc
View File

@ -138,11 +138,11 @@ TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
const int num_submaps_to_keep = 3; const int num_submaps_to_keep = 3;
const int num_nodes_per_submap = 2; const int num_nodes_per_submap = 2;
for (int i = 0; i < num_submaps_to_create; ++i) { for (int i = 0; i < num_submaps_to_create; ++i) {
int submap_index = 42 + i; int submap_index = (i < 3) ? 42 + i : 100 + i;
auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index); auto submap = test::CreateFakeSubmap3D(trajectory_id, submap_index);
pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap); pose_graph_->AddSubmapFromProto(transform::Rigid3d::Identity(), submap);
for (int j = 0; j < num_nodes_per_submap; ++j) { for (int j = 0; j < num_nodes_per_submap; ++j) {
int node_index = 7 + num_nodes_per_submap * i + j; int node_index = 7 + num_nodes_per_submap * submap_index + j;
auto node = test::CreateFakeNode(trajectory_id, node_index); auto node = test::CreateFakeNode(trajectory_id, node_index);
pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node); pose_graph_->AddNodeFromProto(transform::Rigid3d::Identity(), node);
proto::PoseGraph proto; proto::PoseGraph proto;
@ -190,6 +190,83 @@ TEST_F(PoseGraph3DTest, PureLocalizationTrimmer) {
} }
} }
class EvenSubmapTrimmer : public PoseGraphTrimmer {
public:
explicit EvenSubmapTrimmer(int trajectory_id)
: trajectory_id_(trajectory_id) {}
void Trim(Trimmable* pose_graph) override {
auto submap_ids = pose_graph->GetSubmapIds(trajectory_id_);
for (const auto submap_id : submap_ids) {
if (submap_id.submap_index % 2 == 0) {
pose_graph->MarkSubmapAsTrimmed(submap_id);
}
}
}
bool IsFinished() override { return false; }
private:
int trajectory_id_;
};
TEST_F(PoseGraph3DTest, EvenSubmapTrimmer) {
BuildPoseGraphWithFakeOptimization();
const int trajectory_id = 2;
const int num_submaps_to_keep = 10;
const int num_submaps_to_create = 2 * num_submaps_to_keep;
const int num_nodes_per_submap = 3;
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);
constraint.set_tag(proto::PoseGraph::Constraint::INTRA_SUBMAP);
test::AddToProtoGraph(constraint, &proto);
pose_graph_->AddSerializedConstraints(FromProto(proto.constraint()));
}
}
pose_graph_->AddTrimmer(
common::make_unique<EvenSubmapTrimmer>(trajectory_id));
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
} // namespace mapping } // namespace mapping
} // namespace cartographer } // namespace cartographer