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

Add 3D global SLAM grpc test. (#904)

master
gaschler 2018-02-14 12:04:52 +01:00 committed by Wally B. Feed
parent 244cf615f5
commit 9793542957
2 changed files with 57 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cartographer/internal/mapping/test_helpers.cc
View File

@ -43,9 +43,11 @@ GenerateFakeRangeMeasurements(double travel_distance, double duration,
std::vector<cartographer::sensor::TimedPointCloudData> 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;

60
cartographer_grpc/client_server_test.cc
View File

@ -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<SensorId> expected_sensor_ids = {kSensorId};
std::set<SensorId> 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<SensorId> expected_sensor_ids = {kSensorId};
std::set<SensorId> 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<const cartographer::sensor::ImuData&>(_)))
.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