From 29f6ea9ea31c649d313d7ba21c0f514ccf8a7db7 Mon Sep 17 00:00:00 2001 From: gaschler Date: Tue, 12 Jun 2018 12:49:07 +0200 Subject: [PATCH] Test LoadState and pure localization (#1190) --- .../mapping/internal/testing/test_helpers.cc | 18 +- .../mapping/internal/testing/test_helpers.h | 5 + cartographer/mapping/map_builder_test.cc | 168 ++++++++++++++++++ 3 files changed, 187 insertions(+), 4 deletions(-) diff --git a/cartographer/mapping/internal/testing/test_helpers.cc b/cartographer/mapping/internal/testing/test_helpers.cc index a104fb6..e34ee34 100644 --- a/cartographer/mapping/internal/testing/test_helpers.cc +++ b/cartographer/mapping/internal/testing/test_helpers.cc @@ -41,6 +41,16 @@ ResolveLuaParameters(const std::string& lua_code) { std::vector GenerateFakeRangeMeasurements(double travel_distance, double duration, double time_step) { + const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized(); + return GenerateFakeRangeMeasurements(kDirection * travel_distance, duration, + time_step, + transform::Rigid3f::Identity()); +} + +std::vector +GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation, + double duration, double time_step, + const transform::Rigid3f& local_to_global) { std::vector measurements; cartographer::sensor::TimedPointCloud point_cloud; for (double angle = 0.; angle < M_PI; angle += 0.01) { @@ -50,18 +60,18 @@ GenerateFakeRangeMeasurements(double travel_distance, double duration, kRadius * std::sin(angle), height, 0.); } } - const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized(); - const Eigen::Vector3f kVelocity = travel_distance / duration * kDirection; + const Eigen::Vector3f kVelocity = translation / duration; for (double elapsed_time = 0.; elapsed_time < duration; elapsed_time += time_step) { cartographer::common::Time time = cartographer::common::FromUniversal(123) + cartographer::common::FromSeconds(elapsed_time); - cartographer::transform::Rigid3f pose = + cartographer::transform::Rigid3f global_pose = + local_to_global * cartographer::transform::Rigid3f::Translation(elapsed_time * kVelocity); cartographer::sensor::TimedPointCloud ranges = cartographer::sensor::TransformTimedPointCloud(point_cloud, - pose.inverse()); + global_pose.inverse()); measurements.emplace_back(cartographer::sensor::TimedPointCloudData{ time, Eigen::Vector3f::Zero(), ranges}); } diff --git a/cartographer/mapping/internal/testing/test_helpers.h b/cartographer/mapping/internal/testing/test_helpers.h index 5b0f2d1..37cd3ef 100644 --- a/cartographer/mapping/internal/testing/test_helpers.h +++ b/cartographer/mapping/internal/testing/test_helpers.h @@ -34,6 +34,11 @@ std::vector GenerateFakeRangeMeasurements(double travel_distance, double duration, double time_step); +std::vector +GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation, + double duration, double time_step, + const transform::Rigid3f& local_to_global); + proto::Submap CreateFakeSubmap3D(int trajectory_id = 1, int submap_index = 1); proto::Node CreateFakeNode(int trajectory_id = 1, int node_index = 1); diff --git a/cartographer/mapping/map_builder_test.cc b/cartographer/mapping/map_builder_test.cc index 3c29164..bd59505 100644 --- a/cartographer/mapping/map_builder_test.cc +++ b/cartographer/mapping/map_builder_test.cc @@ -17,7 +17,9 @@ #include "cartographer/mapping/map_builder.h" #include "cartographer/common/config.h" +#include "cartographer/io/proto_stream.h" #include "cartographer/mapping/internal/testing/test_helpers.h" +#include "gmock/gmock.h" #include "gtest/gtest.h" namespace cartographer { @@ -40,6 +42,8 @@ class MapBuilderTest : public ::testing::Test { include "map_builder.lua" MAP_BUILDER.use_trajectory_builder_2d = true MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0 + MAP_BUILDER.pose_graph.global_sampling_ratio = 0.05 + MAP_BUILDER.pose_graph.global_constraint_search_after_n_seconds = 0 return MAP_BUILDER)text"; auto map_builder_parameters = test::ResolveLuaParameters(kMapBuilderLua); map_builder_options_ = @@ -85,6 +89,21 @@ class MapBuilderTest : public ::testing::Test { }; } + int CreateTrajectoryWithFakeData() { + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + auto measurements = test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + } + map_builder_->FinishTrajectory(trajectory_id); + return trajectory_id; + } + std::unique_ptr map_builder_; proto::MapBuilderOptions map_builder_options_; proto::TrajectoryBuilderOptions trajectory_builder_options_; @@ -189,6 +208,10 @@ TEST_F(MapBuilderTest, GlobalSlam2D) { .norm(), 0.1 * kTravelDistance); EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50); + EXPECT_THAT(map_builder_->pose_graph()->constraints(), + testing::Contains(testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))); const auto trajectory_nodes = map_builder_->pose_graph()->GetTrajectoryNodes(); EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 20); @@ -228,6 +251,10 @@ TEST_F(MapBuilderTest, GlobalSlam3D) { .norm(), 0.1 * kTravelDistance); EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 10); + EXPECT_THAT(map_builder_->pose_graph()->constraints(), + testing::Contains(testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))); const auto trajectory_nodes = map_builder_->pose_graph()->GetTrajectoryNodes(); EXPECT_GE(trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 5); @@ -240,6 +267,147 @@ TEST_F(MapBuilderTest, GlobalSlam3D) { 0.1 * kTravelDistance); } +TEST_F(MapBuilderTest, SaveLoadState) { + for (int dimensions : {2, 3}) { + if (dimensions == 3) { + SetOptionsTo3D(); + } + trajectory_builder_options_.mutable_trajectory_builder_2d_options() + ->set_use_imu_data(true); + BuildMapBuilder(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId, kIMUSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + const auto measurements = test::GenerateFakeRangeMeasurements( + kTravelDistance, kDuration, kTimeStep); + for (const auto& measurement : measurements) { + trajectory_builder->AddSensorData(kRangeSensorId.id, measurement); + trajectory_builder->AddSensorData( + kIMUSensorId.id, + sensor::ImuData{measurement.time, Eigen::Vector3d(0., 0., 9.8), + Eigen::Vector3d::Zero()}); + } + map_builder_->FinishTrajectory(trajectory_id); + map_builder_->pose_graph()->RunFinalOptimization(); + int num_constraints = map_builder_->pose_graph()->constraints().size(); + int num_nodes = + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + trajectory_id); + EXPECT_GT(num_constraints, 0); + EXPECT_GT(num_nodes, 0); + // TODO(gaschler): Consider using in-memory to avoid side effects. + const std::string filename = "temp-SaveLoadState.pbstream"; + io::ProtoStreamWriter writer(filename); + map_builder_->SerializeState(&writer); + writer.Close(); + + // Reset 'map_builder_'. + BuildMapBuilder(); + io::ProtoStreamReader reader(filename); + map_builder_->LoadState(&reader, false /* load_frozen_state */); + // TODO(gaschler): Design better way to find out which new trajectory_ids + // were created by LoadState. + map_builder_->pose_graph()->RunFinalOptimization(); + auto nodes = map_builder_->pose_graph()->GetTrajectoryNodes(); + ASSERT_GT(nodes.size(), 0); + int new_trajectory_id = nodes.begin()->id.trajectory_id; + EXPECT_EQ(num_constraints, + map_builder_->pose_graph()->constraints().size()); + EXPECT_EQ( + num_nodes, + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + new_trajectory_id)); + } +} + +TEST_F(MapBuilderTest, LocalizationOnFrozenTrajectory2D) { + BuildMapBuilder(); + int temp_trajectory_id = CreateTrajectoryWithFakeData(); + map_builder_->pose_graph()->RunFinalOptimization(); + EXPECT_GT(map_builder_->pose_graph()->constraints().size(), 0); + EXPECT_GT( + map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero( + temp_trajectory_id), + 0); + const std::string filename = "temp-LocalizationOnFrozenTrajectory2D.pbstream"; + io::ProtoStreamWriter writer(filename); + map_builder_->SerializeState(&writer); + writer.Close(); + + // Reset 'map_builder_'. + local_slam_result_poses_.clear(); + SetOptionsEnableGlobalOptimization(); + BuildMapBuilder(); + io::ProtoStreamReader reader(filename); + map_builder_->LoadState(&reader, true /* load_frozen_state */); + map_builder_->pose_graph()->RunFinalOptimization(); + int trajectory_id = map_builder_->AddTrajectoryBuilder( + {kRangeSensorId}, trajectory_builder_options_, + GetLocalSlamResultCallback()); + TrajectoryBuilderInterface* trajectory_builder = + map_builder_->GetTrajectoryBuilder(trajectory_id); + transform::Rigid3d frozen_trajectory_to_global( + Eigen::Vector3d(0.5, 0.4, 0), + Eigen::Quaterniond(Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitZ()))); + Eigen::Vector3d travel_translation = + Eigen::Vector3d(2., 1., 0.).normalized() * kTravelDistance; + auto measurements = test::GenerateFakeRangeMeasurements( + travel_translation.cast(), kDuration, kTimeStep, + frozen_trajectory_to_global.cast()); + for (auto& measurement : measurements) { + measurement.time += common::FromSeconds(100.); + trajectory_builder->AddSensorData(kRangeSensorId.id, 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.15 * kTravelDistance); + EXPECT_GE(map_builder_->pose_graph()->constraints().size(), 50); + auto constraints = map_builder_->pose_graph()->constraints(); + int num_cross_trajectory_constraints = 0; + for (const auto& constraint : constraints) { + if (constraint.node_id.trajectory_id != + constraint.submap_id.trajectory_id) { + ++num_cross_trajectory_constraints; + } + } + EXPECT_GT(num_cross_trajectory_constraints, 3); + // TODO(gaschler): Subscribe global slam callback, verify that all nodes are + // optimized. + EXPECT_THAT(constraints, testing::Contains(testing::Field( + &PoseGraphInterface::Constraint::tag, + PoseGraphInterface::Constraint::INTER_SUBMAP))); + 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 global_pose = + map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id) * + local_slam_result_poses_.back(); + EXPECT_NEAR(frozen_trajectory_to_global.translation().norm(), + map_builder_->pose_graph() + ->GetLocalToGlobalTransform(trajectory_id) + .translation() + .norm(), + 0.1); + const transform::Rigid3d expected_global_pose = + frozen_trajectory_to_global * + transform::Rigid3d::Translation(travel_translation); + EXPECT_NEAR( + 0., + (global_pose.translation() - expected_global_pose.translation()).norm(), + 0.3) + << "global_pose: " << global_pose + << "expected_global_pose: " << expected_global_pose; +} + } // namespace } // namespace mapping } // namespace cartographer