diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 9c3c994..5525f2e 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -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 GenerateFakeRangeMeasurements() { std::vector 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 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