From 979354295776587d235b7fe6718026c0be3e1ea1 Mon Sep 17 00:00:00 2001 From: gaschler Date: Wed, 14 Feb 2018 12:04:52 +0100 Subject: [PATCH] Add 3D global SLAM grpc test. (#904) --- cartographer/internal/mapping/test_helpers.cc | 8 ++- cartographer_grpc/client_server_test.cc | 60 ++++++++++++++++--- 2 files changed, 57 insertions(+), 11 deletions(-) diff --git a/cartographer/internal/mapping/test_helpers.cc b/cartographer/internal/mapping/test_helpers.cc index 6b31b6e..1f0fd68 100644 --- a/cartographer/internal/mapping/test_helpers.cc +++ b/cartographer/internal/mapping/test_helpers.cc @@ -43,9 +43,11 @@ GenerateFakeRangeMeasurements(double travel_distance, double duration, std::vector measurements; cartographer::sensor::TimedPointCloud point_cloud; for (double angle = 0.; angle < M_PI; angle += 0.01) { - constexpr double kRadius = 5; - point_cloud.emplace_back(kRadius * std::cos(angle), - kRadius * std::sin(angle), 0., 0.); + for (double height : {-0.4, -0.2, 0.0, 0.2, 0.4}) { + constexpr double kRadius = 5; + point_cloud.emplace_back(kRadius * std::cos(angle), + kRadius * std::sin(angle), height, 0.); + } } const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized(); const Eigen::Vector3f kVelocity = travel_distance / duration * kDirection; diff --git a/cartographer_grpc/client_server_test.cc b/cartographer_grpc/client_server_test.cc index e63fdc6..0e4d52a 100644 --- a/cartographer_grpc/client_server_test.cc +++ b/cartographer_grpc/client_server_test.cc @@ -38,7 +38,7 @@ using testing::_; namespace cartographer_grpc { namespace { -const SensorId kSensorId{SensorId::SensorType::IMU, "sensor"}; +const SensorId kImuSensorId{SensorId::SensorType::IMU, "imu"}; const SensorId kRangeSensorId{SensorId::SensorType::RANGE, "range"}; constexpr double kDuration = 4.; // Seconds. constexpr double kTimeStep = 0.1; // Seconds. @@ -142,7 +142,7 @@ TEST_F(ClientServerTest, AddTrajectoryBuilder) { server_->Start(); InitializeStub(); int trajectory_id = stub_->AddTrajectoryBuilder( - {kSensorId}, trajectory_builder_options_, nullptr); + {kImuSensorId}, trajectory_builder_options_, nullptr); stub_->FinishTrajectory(trajectory_id); server_->Shutdown(); } @@ -151,7 +151,7 @@ TEST_F(ClientServerTest, AddTrajectoryBuilderWithMock) { InitializeServerWithMockMapBuilder(); server_->Start(); InitializeStub(); - std::set expected_sensor_ids = {kSensorId}; + std::set expected_sensor_ids = {kImuSensorId}; EXPECT_CALL( *mock_map_builder_, AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _)) @@ -173,13 +173,13 @@ TEST_F(ClientServerTest, AddSensorData) { server_->Start(); InitializeStub(); int trajectory_id = stub_->AddTrajectoryBuilder( - {kSensorId}, trajectory_builder_options_, nullptr); + {kImuSensorId}, trajectory_builder_options_, nullptr); TrajectoryBuilderInterface* trajectory_stub = stub_->GetTrajectoryBuilder(trajectory_id); cartographer::sensor::ImuData imu_data{ cartographer::common::FromUniversal(42), Eigen::Vector3d(0., 0., 9.8), Eigen::Vector3d::Zero()}; - trajectory_stub->AddSensorData(kSensorId.id, imu_data); + trajectory_stub->AddSensorData(kImuSensorId.id, imu_data); stub_->FinishTrajectory(trajectory_id); server_->Shutdown(); } @@ -188,7 +188,7 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) { InitializeServerWithMockMapBuilder(); server_->Start(); InitializeStub(); - std::set expected_sensor_ids = {kSensorId}; + std::set expected_sensor_ids = {kImuSensorId}; EXPECT_CALL( *mock_map_builder_, AddTrajectoryBuilder(::testing::ContainerEq(expected_sensor_ids), _, _)) @@ -205,10 +205,10 @@ TEST_F(ClientServerTest, AddSensorDataWithMock) { Eigen::Vector3d::Zero()}; EXPECT_CALL(*mock_trajectory_builder_, AddSensorData( - ::testing::StrEq(kSensorId.id), + ::testing::StrEq(kImuSensorId.id), ::testing::Matcher(_))) .WillOnce(::testing::Return()); - trajectory_stub->AddSensorData(kSensorId.id, imu_data); + trajectory_stub->AddSensorData(kImuSensorId.id, imu_data); EXPECT_CALL(*mock_map_builder_, FinishTrajectory(trajectory_id)); stub_->FinishTrajectory(trajectory_id); server_->Shutdown(); @@ -240,5 +240,49 @@ TEST_F(ClientServerTest, LocalSlam2D) { server_->Shutdown(); } +TEST_F(ClientServerTest, GlobalSlam3D) { + map_builder_server_options_.mutable_map_builder_options() + ->set_use_trajectory_builder_2d(false); + map_builder_server_options_.mutable_map_builder_options() + ->set_use_trajectory_builder_3d(true); + map_builder_server_options_.mutable_map_builder_options() + ->mutable_pose_graph_options() + ->set_optimize_every_n_nodes(3); + trajectory_builder_options_.mutable_trajectory_builder_3d_options() + ->mutable_motion_filter_options() + ->set_max_distance_meters(0); + trajectory_builder_options_.mutable_trajectory_builder_3d_options() + ->mutable_motion_filter_options() + ->set_max_angle_radians(0); + InitializeRealServer(); + server_->Start(); + InitializeStub(); + int trajectory_id = stub_->AddTrajectoryBuilder( + {kRangeSensorId, kImuSensorId}, trajectory_builder_options_, + local_slam_result_callback_); + TrajectoryBuilderInterface* trajectory_stub = + stub_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = + cartographer::mapping::test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + cartographer::sensor::ImuData imu_data{ + measurement.time - cartographer::common::FromSeconds(kTimeStep / 2), + Eigen::Vector3d(0., 0., 9.8), Eigen::Vector3d::Zero()}; + trajectory_stub->AddSensorData(kImuSensorId.id, imu_data); + trajectory_stub->AddSensorData(kRangeSensorId.id, measurement); + } + // First range data will not call back while initializing PoseExtrapolator, so + // expect one less. + WaitForLocalSlamResults(measurements.size() - 1); + stub_->FinishTrajectory(trajectory_id); + EXPECT_NEAR(kTravelDistance, + (local_slam_result_poses_.back().translation() - + local_slam_result_poses_.front().translation()) + .norm(), + 0.1 * kTravelDistance); + server_->Shutdown(); +} + } // namespace } // namespace cartographer_grpc