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

Integration tests for local slam. (#734)

master
gaschler 2017-12-06 15:58:16 +01:00 committed by Christoph Schütte
parent ded778cd34
commit 4c999037b4
1 changed files with 143 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

156
cartographer/mapping/map_builder_test.cc
View File

@ -31,6 +31,40 @@ namespace cartographer {
namespace mapping {
namespace {
constexpr char kRangeSensorId[] = "range";
constexpr char kIMUSensorId[] = "imu";
constexpr double kDuration = 2.;
constexpr double kTimeStep = 0.1;
constexpr double kTravelDistance = 0.4;
struct FakeRangeMeasurement {
common::Time time;
sensor::TimedPointCloud ranges;
};
std::vector<FakeRangeMeasurement> GenerateFakeRangeMeasurements() {
std::vector<FakeRangeMeasurement> measurements;
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.);
}
const Eigen::Vector3f kDirection = Eigen::Vector3f(2., 1., 0.).normalized();
const Eigen::Vector3f kVelocity = kTravelDistance / kDuration * kDirection;
for (double elapsed_time = 0.; elapsed_time < kDuration;
elapsed_time += kTimeStep) {
common::Time time =
common::FromUniversal(123) + common::FromSeconds(elapsed_time);
transform::Rigid3f pose =
transform::Rigid3f::Translation(elapsed_time * kVelocity);
sensor::TimedPointCloud ranges =
sensor::TransformTimedPointCloud(point_cloud, pose.inverse());
measurements.emplace_back(FakeRangeMeasurement{time, ranges});
}
return measurements;
}
std::unique_ptr<::cartographer::common::LuaParameterDictionary>
ResolveLuaParameters(const std::string& lua_code) {
auto file_resolver = ::cartographer::common::make_unique<
@ -45,40 +79,134 @@ ResolveLuaParameters(const std::string& lua_code) {
class MapBuilderTest : public ::testing::Test {
protected:
void SetUp() override {
// Global SLAM optimization is not executed.
const std::string kMapBuilderLua = R"text(
include "map_builder.lua"
MAP_BUILDER.use_trajectory_builder_2d = true
MAP_BUILDER.pose_graph.optimize_every_n_nodes = 0
return MAP_BUILDER)text";
auto parameter_dictionary = ResolveLuaParameters(kMapBuilderLua);
proto::MapBuilderOptions options =
CreateMapBuilderOptions(parameter_dictionary.get());
map_builder_ = common::make_unique<MapBuilder>(options);
}
std::unique_ptr<MapBuilderInterface> map_builder_;
};
TEST_F(MapBuilderTest, TrajectoryAddFinish) {
const std::string kRangeSensorId = "lidar";
auto map_builder_parameters = ResolveLuaParameters(kMapBuilderLua);
map_builder_options_ =
CreateMapBuilderOptions(map_builder_parameters.get());
// Multiple submaps are created because of a small 'num_range_data'.
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
return TRAJECTORY_BUILDER)text";
const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
auto trajectory_builder_parameters =
ResolveLuaParameters(kTrajectoryBuilderLua);
proto::TrajectoryBuilderOptions trajectory_options =
trajectory_builder_options_ =
CreateTrajectoryBuilderOptions(trajectory_builder_parameters.get());
}
void BuildMapBuilder() {
map_builder_ = common::make_unique<MapBuilder>(map_builder_options_);
}
void SetOptionsTo3D() {
map_builder_options_.set_use_trajectory_builder_2d(false);
map_builder_options_.set_use_trajectory_builder_3d(true);
}
MapBuilderInterface::LocalSlamResultCallback GetLocalSlamResultCallback() {
return [=](const int trajectory_id, const ::cartographer::common::Time time,
const ::cartographer::transform::Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<const ::cartographer::mapping::NodeId>) {
local_slam_result_poses_.push_back(local_pose);
};
}
std::unique_ptr<MapBuilderInterface> map_builder_;
proto::MapBuilderOptions map_builder_options_;
proto::TrajectoryBuilderOptions trajectory_builder_options_;
std::vector<::cartographer::transform::Rigid3d> local_slam_result_poses_;
};
TEST_F(MapBuilderTest, TrajectoryAddFinish2D) {
BuildMapBuilder();
const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options,
expected_sensor_ids, trajectory_builder_options_,
nullptr /* local_slam_result_callback */);
EXPECT_EQ(1, map_builder_->num_trajectory_builders());
EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr);
EXPECT_TRUE(map_builder_->pose_graph() != nullptr);
map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization();
EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id));
}
TEST_F(MapBuilderTest, TrajectoryAddFinish3D) {
SetOptionsTo3D();
BuildMapBuilder();
const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_,
nullptr /* local_slam_result_callback */);
EXPECT_EQ(1, map_builder_->num_trajectory_builders());
EXPECT_TRUE(map_builder_->GetTrajectoryBuilder(trajectory_id) != nullptr);
EXPECT_TRUE(map_builder_->pose_graph() != nullptr);
map_builder_->FinishTrajectory(trajectory_id);
map_builder_->pose_graph()->RunFinalOptimization();
EXPECT_TRUE(map_builder_->pose_graph()->IsTrajectoryFinished(trajectory_id));
}
TEST_F(MapBuilderTest, LocalSlam2D) {
BuildMapBuilder();
const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId};
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback());
TrajectoryBuilder* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) {
trajectory_builder->AddRangefinderData(kRangeSensorId, measurement.time,
Eigen::Vector3f::Zero(),
measurement.ranges);
}
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);
}
TEST_F(MapBuilderTest, LocalSlam3D) {
SetOptionsTo3D();
BuildMapBuilder();
const std::unordered_set<std::string> expected_sensor_ids = {kRangeSensorId,
kIMUSensorId};
int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_builder_options_,
GetLocalSlamResultCallback());
TrajectoryBuilder* trajectory_builder =
map_builder_->GetTrajectoryBuilder(trajectory_id);
const auto measurements = GenerateFakeRangeMeasurements();
for (const auto& measurement : measurements) {
trajectory_builder->AddRangefinderData(kRangeSensorId, measurement.time,
Eigen::Vector3f::Zero(),
measurement.ranges);
trajectory_builder->AddImuData(kIMUSensorId, measurement.time,
Eigen::Vector3d(0., 0., 9.8),
Eigen::Vector3d::Zero());
}
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);
}
} // namespace
} // namespace mapping
} // namespace cartographer