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

Test LoadState and pure localization (#1190)

master
gaschler 2018-06-12 12:49:07 +02:00 committed by Wally B. Feed
parent 5055703490
commit 29f6ea9ea3
3 changed files with 187 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cartographer/mapping/internal/testing/test_helpers.cc
View File

@ -41,6 +41,16 @@ ResolveLuaParameters(const std::string& lua_code) {
std::vector<cartographer::sensor::TimedPointCloudData>
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<cartographer::sensor::TimedPointCloudData>
GenerateFakeRangeMeasurements(const Eigen::Vector3f& translation,
double duration, double time_step,
const transform::Rigid3f& local_to_global) {
std::vector<cartographer::sensor::TimedPointCloudData> 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});
}

5
cartographer/mapping/internal/testing/test_helpers.h
View File

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

168
cartographer/mapping/map_builder_test.cc
View File

@ -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<MapBuilderInterface> 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<float>(), kDuration, kTimeStep,
frozen_trajectory_to_global.cast<float>());
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