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

Moves 2D laser options into 2D trajectory builder. (#114)

master
Damon Kohler 2016-11-15 13:24:58 +01:00 committed by GitHub
parent 1de387cba9
commit 8cd3178c69
7 changed files with 79 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
cartographer/mapping_2d/local_trajectory_builder.cc
View File

@ -27,8 +27,14 @@ namespace mapping_2d {
proto::LocalTrajectoryBuilderOptions CreateLocalTrajectoryBuilderOptions(
common::LuaParameterDictionary* const parameter_dictionary) {
proto::LocalTrajectoryBuilderOptions options;
options.set_laser_min_range(
parameter_dictionary->GetDouble("laser_min_range"));
options.set_laser_max_range(
parameter_dictionary->GetDouble("laser_max_range"));
options.set_laser_min_z(parameter_dictionary->GetDouble("laser_min_z"));
options.set_laser_max_z(parameter_dictionary->GetDouble("laser_max_z"));
options.set_laser_missing_echo_ray_length(
parameter_dictionary->GetDouble("laser_missing_echo_ray_length"));
options.set_laser_voxel_filter_size(
parameter_dictionary->GetDouble("laser_voxel_filter_size"));
options.set_use_online_correlative_scan_matching(
@ -70,17 +76,33 @@ LocalTrajectoryBuilder::~LocalTrajectoryBuilder() {}
const Submaps* LocalTrajectoryBuilder::submaps() const { return &submaps_; }
sensor::LaserFan LocalTrajectoryBuilder::BuildCroppedLaserFan(
sensor::LaserFan LocalTrajectoryBuilder::TransformAndFilterLaserFan(
const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::LaserFan& laser_fan) const {
const sensor::LaserFan cropped_fan = sensor::CropLaserFan(
sensor::TransformLaserFan(laser_fan, tracking_to_tracking_2d),
// Drop any returns below the minimum range and convert returns beyond the
// maximum range into misses.
sensor::LaserFan returns_and_misses{laser_fan.origin, {}, {}, {}};
for (const Eigen::Vector3f& return_ : laser_fan.returns) {
const float range = (return_ - laser_fan.origin).norm();
if (range >= options_.laser_min_range()) {
if (range <= options_.laser_max_range()) {
returns_and_misses.returns.push_back(return_);
} else {
returns_and_misses.misses.push_back(
laser_fan.origin +
options_.laser_missing_echo_ray_length() *
(return_ - laser_fan.origin).normalized());
}
}
}
const sensor::LaserFan cropped = sensor::CropLaserFan(
sensor::TransformLaserFan(returns_and_misses, tracking_to_tracking_2d),
options_.laser_min_z(), options_.laser_max_z());
return sensor::LaserFan{
cropped_fan.origin,
sensor::VoxelFiltered(cropped_fan.returns,
cropped.origin,
sensor::VoxelFiltered(cropped.returns,
options_.laser_voxel_filter_size()),
sensor::VoxelFiltered(cropped_fan.misses,
sensor::VoxelFiltered(cropped.misses,
options_.laser_voxel_filter_size())};
}
@ -157,8 +179,8 @@ LocalTrajectoryBuilder::AddHorizontalLaserFan(
-transform::GetYaw(pose_prediction), Eigen::Vector3d::UnitZ())) *
pose_prediction.rotation());
const sensor::LaserFan laser_fan_in_tracking_2d =
BuildCroppedLaserFan(tracking_to_tracking_2d.cast<float>(), laser_fan);
const sensor::LaserFan laser_fan_in_tracking_2d = TransformAndFilterLaserFan(
tracking_to_tracking_2d.cast<float>(), laser_fan);
if (laser_fan_in_tracking_2d.returns.empty()) {
LOG(WARNING) << "Dropped empty horizontal laser point cloud.";

3
cartographer/mapping_2d/local_trajectory_builder.h
View File

@ -73,8 +73,7 @@ class LocalTrajectoryBuilder {
const Submaps* submaps() const;
private:
// Transforms 'laser_scan', crops and voxel filters.
sensor::LaserFan BuildCroppedLaserFan(
sensor::LaserFan TransformAndFilterLaserFan(
const transform::Rigid3f& tracking_to_tracking_2d,
const sensor::LaserFan& laser_fan) const;

8
cartographer/mapping_2d/proto/local_trajectory_builder_options.proto
View File

@ -24,10 +24,16 @@ import "cartographer/mapping_2d/scan_matching/proto/ceres_scan_matcher_options.p
import "cartographer/mapping_2d/scan_matching/proto/real_time_correlative_scan_matcher_options.proto";
message LocalTrajectoryBuilderOptions {
// Cropping parameters for the laser fan.
// Laser returns outside these ranges will be dropped.
optional float laser_min_range = 14;
optional float laser_max_range = 15;
optional float laser_min_z = 1;
optional float laser_max_z = 2;
// Laser returns beyond 'laser_max_range' will be inserted with this length as
// empty space.
optional float laser_missing_echo_ray_length = 16;
// Voxel filter that gets applied to the horizontal laser immediately after
// cropping.
optional float laser_voxel_filter_size = 3;

20
cartographer/sensor/laser.cc
View File

@ -37,31 +37,25 @@ std::vector<uint8> ReorderReflectivities(
} // namespace
LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range,
const float max_range,
const float missing_echo_ray_length) {
CHECK_GE(min_range, 0.f);
PointCloud ToPointCloud(const proto::LaserScan& proto) {
CHECK_GE(proto.range_min(), 0.f);
CHECK_GE(proto.range_max(), proto.range_min());
CHECK_GT(proto.angle_increment(), 0.f);
CHECK_GT(proto.angle_max(), proto.angle_min());
LaserFan laser_fan = {Eigen::Vector3f::Zero(), {}, {}};
PointCloud point_cloud;
float angle = proto.angle_min();
for (const auto& range : proto.range()) {
if (range.value_size() > 0) {
const float first_echo = range.value(0);
if (!std::isnan(first_echo) && first_echo >= min_range) {
if (proto.range_min() <= first_echo && first_echo <= proto.range_max()) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
if (first_echo <= max_range) {
laser_fan.returns.push_back(rotation *
point_cloud.push_back(rotation *
(first_echo * Eigen::Vector3f::UnitX()));
} else {
laser_fan.misses.push_back(
rotation * (missing_echo_ray_length * Eigen::Vector3f::UnitX()));
}
}
}
angle += proto.angle_increment();
}
return laser_fan;
return point_cloud;
}
proto::LaserFan ToProto(const LaserFan& laser_fan) {

11
cartographer/sensor/laser.h
View File

@ -38,14 +38,11 @@ struct LaserFan {
std::vector<uint8> reflectivities;
};
// Builds a LaserFan from 'proto' and separates any beams with ranges outside
// the range ['min_range', 'max_range']. Beams beyond 'max_range' are inserted
// into the 'misses' point cloud with length 'missing_echo_ray_length'. The
// points in both clouds are stored in scan order.
LaserFan ToLaserFan(const proto::LaserScan& proto, float min_range,
float max_range, float missing_echo_ray_length);
// Builds a PointCloud of returns from 'proto', dropping any beams with ranges
// outside the valid range described by 'proto'.
PointCloud ToPointCloud(const proto::LaserScan& proto);
// Converts 3D 'laser_fan' to a proto::LaserFan.
// Converts 'laser_fan' to a proto::LaserFan.
proto::LaserFan ToProto(const LaserFan& laser_fan);
// Converts 'proto' to a LaserFan.

43
cartographer/sensor/laser_test.cc
View File

@ -28,7 +28,7 @@ namespace {
using ::testing::Contains;
using ::testing::PrintToString;
TEST(ProjectorTest, ToLaserFan) {
TEST(LaserTest, ToPointCloud) {
proto::LaserScan laser_scan;
for (int i = 0; i < 8; ++i) {
laser_scan.add_range()->add_value(1.f);
@ -36,24 +36,26 @@ TEST(ProjectorTest, ToLaserFan) {
laser_scan.set_angle_min(0.f);
laser_scan.set_angle_max(8.f * static_cast<float>(M_PI_4));
laser_scan.set_angle_increment(static_cast<float>(M_PI_4));
laser_scan.set_range_min(0.f);
laser_scan.set_range_max(10.f);
const LaserFan fan = ToLaserFan(laser_scan, 0.f, 10.f, 1.f);
EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[1].isApprox(
const auto point_cloud = ToPointCloud(laser_scan);
EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(point_cloud[1].isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
EXPECT_TRUE(fan.returns[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[3].isApprox(
EXPECT_TRUE(point_cloud[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
EXPECT_TRUE(point_cloud[3].isApprox(
Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
EXPECT_TRUE(fan.returns[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[5].isApprox(
EXPECT_TRUE(point_cloud[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(point_cloud[5].isApprox(
Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
1e-6));
EXPECT_TRUE(fan.returns[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[7].isApprox(
EXPECT_TRUE(point_cloud[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
EXPECT_TRUE(point_cloud[7].isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6));
}
TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) {
TEST(LaserTest, ToPointCloudWithInfinityAndNaN) {
proto::LaserScan laser_scan;
laser_scan.add_range()->add_value(1.f);
laser_scan.add_range()->add_value(std::numeric_limits<float>::infinity());
@ -63,15 +65,13 @@ TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) {
laser_scan.set_angle_min(0.f);
laser_scan.set_angle_max(3.f * static_cast<float>(M_PI_4));
laser_scan.set_angle_increment(static_cast<float>(M_PI_4));
laser_scan.set_range_min(2.f);
laser_scan.set_range_max(10.f);
const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f);
ASSERT_EQ(2, fan.returns.size());
EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
EXPECT_TRUE(fan.returns[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
ASSERT_EQ(1, fan.misses.size());
EXPECT_TRUE(fan.misses[0].isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
const auto point_cloud = ToPointCloud(laser_scan);
ASSERT_EQ(2, point_cloud.size());
EXPECT_TRUE(point_cloud[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
EXPECT_TRUE(point_cloud[1].isApprox(Eigen::Vector3f(-3.f, 0.f, 0.f), 1e-6));
}
// Custom matcher for pair<eigen::Vector3f, int> entries.
@ -82,12 +82,13 @@ MATCHER_P(PairApproximatelyEquals, expected,
}
TEST(LaserTest, Compression) {
LaserFan fan = {Eigen::Vector3f(1, 1, 1),
const LaserFan laser_fan = {
Eigen::Vector3f(1, 1, 1),
{Eigen::Vector3f(0, 1, 2), Eigen::Vector3f(4, 5, 6),
Eigen::Vector3f(0, 1, 2)},
{Eigen::Vector3f(7, 8, 9)},
{1, 2, 3}};
LaserFan actual = Decompress(Compress(fan));
const LaserFan actual = Decompress(Compress(laser_fan));
EXPECT_TRUE(actual.origin.isApprox(Eigen::Vector3f(1, 1, 1), 1e-6));
EXPECT_EQ(3, actual.returns.size());
EXPECT_EQ(1, actual.misses.size());

3
configuration_files/trajectory_builder_2d.lua
View File

@ -14,8 +14,11 @@
TRAJECTORY_BUILDER_2D = {
use_imu_data = true,
laser_min_range = 0.,
laser_max_range = 30.,
laser_min_z = -0.8,
laser_max_z = 2.,
laser_missing_echo_ray_length = 5.,
laser_voxel_filter_size = 0.025,
use_online_correlative_scan_matching = false,