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

Test global 2D SLAM. (#751)

master
gaschler 2017-12-15 14:45:56 +01:00 committed by GitHub
parent 69f74a11ba
commit 6817d22788
1 changed files with 47 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
cartographer/mapping/map_builder_test.cc
View File

@ -33,9 +33,9 @@ namespace {
constexpr char kRangeSensorId[] = "range";
constexpr char kIMUSensorId[] = "imu";
constexpr double kDuration = 2.;
constexpr double kTimeStep = 0.1;
constexpr double kTravelDistance = 0.4;
constexpr double kDuration = 4.; // Seconds.
constexpr double kTimeStep = 0.1; // Seconds.
constexpr double kTravelDistance = 1.2; // Meters.
std::vector<sensor::TimedPointCloudData> GenerateFakeRangeMeasurements() {
std::vector<sensor::TimedPointCloudData> measurements;
@ -88,8 +88,8 @@ class MapBuilderTest : public ::testing::Test {
const std::string kTrajectoryBuilderLua = R"text(
include "trajectory_builder.lua"
TRAJECTORY_BUILDER.trajectory_builder_2d.use_imu_data = false
TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 5
TRAJECTORY_BUILDER.trajectory_builder_3d.submaps.num_range_data = 5
TRAJECTORY_BUILDER.trajectory_builder_2d.submaps.num_range_data = 4
TRAJECTORY_BUILDER.trajectory_builder_3d.submaps.num_range_data = 4
return TRAJECTORY_BUILDER)text";
auto trajectory_builder_parameters =
ResolveLuaParameters(kTrajectoryBuilderLua);
@ -106,6 +106,14 @@ class MapBuilderTest : public ::testing::Test {
map_builder_options_.set_use_trajectory_builder_3d(true);
}
void SetOptionsEnableGlobalOptimization() {
map_builder_options_.mutable_pose_graph_options()
->set_optimize_every_n_nodes(3);
trajectory_builder_options_.mutable_trajectory_builder_2d_options()
->mutable_motion_filter_options()
->set_max_distance_meters(0);
}
MapBuilderInterface::LocalSlamResultCallback GetLocalSlamResultCallback() {
return [=](const int trajectory_id, const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
@ -200,6 +208,40 @@ TEST_F(MapBuilderTest, LocalSlam3D) {
0.1 * kTravelDistance);
}
TEST_F(MapBuilderTest, GlobalSlam2D) {
SetOptionsEnableGlobalOptimization();
BuildMapBuilder();
const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback());
TrajectoryBuilderInterface* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) {
trajectory_builder->AddSensorData(kRangeSensorId, measurement);
}
map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization();
EXPECT_EQ(local_slam_result_poses_.size(), measurements.size());
EXPECT_NEAR(kTravelDistance,
(local_slam_result_poses_.back().translation() -
local_slam_result_poses_.front().translation())
.norm(),
0.1 * kTravelDistance);
EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50);
const auto trajectory_nodes =
map_builder_->pose_graph()->GetTrajectoryNodes();
EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 20);
const auto submap_data = map_builder_->pose_graph()->GetAllSubmapData();
EXPECT_GE(submap_data.SizeOfTrajectoryOrZero(trajectory_id), 5);
const transform::Rigid3d final_pose =
map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) *
local_slam_result_poses_.back();
EXPECT_NEAR(kTravelDistance, final_pose.translation().norm(),
0.1 * kTravelDistance);
}
} // namespace
} // namespace mapping
} // namespace cartographer