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

Simplify the conversion of LaserScan to LaserFan3D. (#57)

master
Wolfgang Hess 2016-10-13 15:09:36 +02:00 committed by GitHub
parent 518850999f
commit 7d93ac3302
3 changed files with 109 additions and 116 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
cartographer/sensor/laser.cc
View File

@ -37,41 +37,6 @@ std::vector<uint8> ReorderReflectivities(
} // namespace } // 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);
CHECK_GT(proto.angle_increment(), 0.f);
CHECK_GT(proto.angle_max(), proto.angle_min());
LaserFan laser_fan = {Eigen::Vector2f::Zero(), {}, {}};
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 (first_echo <= max_range) {
laser_fan.point_cloud.push_back(Eigen::Rotation2Df(angle) *
Eigen::Vector2f(first_echo, 0.f));
} else {
laser_fan.missing_echo_point_cloud.push_back(
Eigen::Rotation2Df(angle) *
Eigen::Vector2f(missing_echo_ray_length, 0.f));
}
}
}
angle += proto.angle_increment();
}
return laser_fan;
}
LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
const Eigen::Vector3f& min,
const Eigen::Vector3f& max) {
return LaserFan{laser_fan.origin.head<2>(),
ProjectToPointCloud2D(Crop(laser_fan.returns, min, max)),
ProjectToPointCloud2D(Crop(laser_fan.misses, min, max))};
}
LaserFan TransformLaserFan(const LaserFan& laser_fan, LaserFan TransformLaserFan(const LaserFan& laser_fan,
const transform::Rigid2f& transform) { const transform::Rigid2f& transform) {
return LaserFan{ return LaserFan{
@ -80,39 +45,31 @@ LaserFan TransformLaserFan(const LaserFan& laser_fan,
TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)}; TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)};
} }
LaserFan3D ToLaserFan3D(const LaserFan& laser_fan) { LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, const float min_range,
return LaserFan3D{ const float max_range,
Eigen::Vector3f(laser_fan.origin.x(), laser_fan.origin.y(), 0.), const float missing_echo_ray_length) {
ToPointCloud(laser_fan.point_cloud), CHECK_GE(min_range, 0.f);
ToPointCloud(laser_fan.missing_echo_point_cloud)}; CHECK_GT(proto.angle_increment(), 0.f);
} CHECK_GT(proto.angle_max(), proto.angle_min());
LaserFan3D laser_fan = {Eigen::Vector3f::Zero(), {}, {}};
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan) { float angle = proto.angle_min();
return LaserFan3D{compressed_laser_fan.origin, for (const auto& range : proto.range()) {
compressed_laser_fan.returns.Decompress(), if (range.value_size() > 0) {
compressed_laser_fan.misses.Decompress(), const float first_echo = range.value(0);
compressed_laser_fan.reflectivities}; if (!std::isnan(first_echo) && first_echo >= min_range) {
} const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
if (first_echo <= max_range) {
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) { laser_fan.returns.push_back(rotation *
std::vector<int> new_to_old; (first_echo * Eigen::Vector3f::UnitX()));
CompressedPointCloud compressed_returns = } else {
CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns, laser_fan.misses.push_back(
&new_to_old); rotation * (missing_echo_ray_length * Eigen::Vector3f::UnitX()));
return CompressedLaserFan3D{ }
laser_fan.origin, std::move(compressed_returns), }
CompressedPointCloud(laser_fan.misses), }
ReorderReflectivities(laser_fan.reflectivities, new_to_old)}; angle += proto.angle_increment();
} }
return laser_fan;
LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
const transform::Rigid3f& transform) {
return LaserFan3D{
transform * laser_fan.origin,
TransformPointCloud(laser_fan.returns, transform),
TransformPointCloud(laser_fan.misses, transform),
laser_fan.reflectivities,
};
} }
proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) { proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) {
@ -135,6 +92,16 @@ LaserFan3D FromProto(const proto::LaserFan3D& proto) {
return laser_fan_3d; return laser_fan_3d;
} }
LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
const transform::Rigid3f& transform) {
return LaserFan3D{
transform * laser_fan.origin,
TransformPointCloud(laser_fan.returns, transform),
TransformPointCloud(laser_fan.misses, transform),
laser_fan.reflectivities,
};
}
LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan, LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
const float max_range) { const float max_range) {
LaserFan3D result{laser_fan.origin, {}, {}, {}}; LaserFan3D result{laser_fan.origin, {}, {}, {}};
@ -146,5 +113,31 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
return result; return result;
} }
LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
const Eigen::Vector3f& min,
const Eigen::Vector3f& max) {
return LaserFan{laser_fan.origin.head<2>(),
ProjectToPointCloud2D(Crop(laser_fan.returns, min, max)),
ProjectToPointCloud2D(Crop(laser_fan.misses, min, max))};
}
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan) {
std::vector<int> new_to_old;
CompressedPointCloud compressed_returns =
CompressedPointCloud::CompressAndReturnOrder(laser_fan.returns,
&new_to_old);
return CompressedLaserFan3D{
laser_fan.origin, std::move(compressed_returns),
CompressedPointCloud(laser_fan.misses),
ReorderReflectivities(laser_fan.reflectivities, new_to_old)};
}
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan) {
return LaserFan3D{compressed_laser_fan.origin,
compressed_laser_fan.returns.Decompress(),
compressed_laser_fan.misses.Decompress(),
compressed_laser_fan.reflectivities};
}
} // namespace sensor } // namespace sensor
} // namespace cartographer } // namespace cartographer

51
cartographer/sensor/laser.h
View File

@ -34,8 +34,6 @@ struct LaserFan {
PointCloud2D point_cloud; PointCloud2D point_cloud;
PointCloud2D missing_echo_point_cloud; PointCloud2D missing_echo_point_cloud;
}; };
LaserFan ToLaserFan(const proto::LaserScan& proto, float min_range,
float max_range, float missing_echo_ray_length);
// Transforms 'laser_fan' according to 'transform'. // Transforms 'laser_fan' according to 'transform'.
LaserFan TransformLaserFan(const LaserFan& laser_fan, LaserFan TransformLaserFan(const LaserFan& laser_fan,
@ -54,6 +52,30 @@ struct LaserFan3D {
std::vector<uint8> reflectivities; std::vector<uint8> reflectivities;
}; };
LaserFan3D ToLaserFan3D(const proto::LaserScan& proto, float min_range,
float max_range, float missing_echo_ray_length);
// Converts 3D 'laser_fan' to a proto::LaserFan3D.
proto::LaserFan3D ToProto(const LaserFan3D& laser_fan);
// Converts 'proto' to a LaserFan3D.
LaserFan3D FromProto(const proto::LaserFan3D& proto);
LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
const transform::Rigid3f& transform);
// Filter a 'laser_fan', retaining only the returns that have no more than
// 'max_range' distance from the laser origin. Removes misses and reflectivity
// information.
LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
float max_range);
// Projects 'laser_fan' into 2D and crops it according to the cuboid defined by
// 'min' and 'max'.
LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
const Eigen::Vector3f& min,
const Eigen::Vector3f& max);
// Like LaserFan3D but with compressed point clouds. The point order changes // Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D. // when converting from LaserFan3D.
struct CompressedLaserFan3D { struct CompressedLaserFan3D {
@ -65,32 +87,9 @@ struct CompressedLaserFan3D {
std::vector<uint8> reflectivities; std::vector<uint8> reflectivities;
}; };
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan); CompressedLaserFan3D Compress(const LaserFan3D& laser_fan);
// Converts 3D 'laser_fan' to a proto::LaserFan3D. LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);
proto::LaserFan3D ToProto(const LaserFan3D& laser_fan);
// Converts 'proto' to a LaserFan3D.
LaserFan3D FromProto(const proto::LaserFan3D& proto);
LaserFan3D ToLaserFan3D(const LaserFan& laser_fan);
LaserFan3D TransformLaserFan3D(const LaserFan3D& laser_fan,
const transform::Rigid3f& transform);
// Projects 'laser_fan' into 2D and crops it according to the cuboid defined by
// 'min' and 'max'.
LaserFan ProjectCroppedLaserFan(const LaserFan3D& laser_fan,
const Eigen::Vector3f& min,
const Eigen::Vector3f& max);
// Filter a 'laser_fan', retaining only the returns that have no more than
// 'max_range' distance from the laser origin. Removes misses and reflectivity
// information.
LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
float max_range);
} // namespace sensor } // namespace sensor
} // namespace cartographer } // namespace cartographer

45
cartographer/sensor/laser_test.cc
View File

@ -28,7 +28,7 @@ namespace {
using ::testing::Contains; using ::testing::Contains;
using ::testing::PrintToString; using ::testing::PrintToString;
TEST(ProjectorTest, ToLaserFan) { TEST(ProjectorTest, ToLaserFan3D) {
proto::LaserScan laser_scan; proto::LaserScan laser_scan;
for (int i = 0; i < 8; ++i) { for (int i = 0; i < 8; ++i) {
laser_scan.add_range()->add_value(1.f); laser_scan.add_range()->add_value(1.f);
@ -37,22 +37,23 @@ TEST(ProjectorTest, ToLaserFan) {
laser_scan.set_angle_max(8.f * static_cast<float>(M_PI_4)); 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_angle_increment(static_cast<float>(M_PI_4));
const LaserFan fan = ToLaserFan(laser_scan, 0.f, 10.f, 1.f); const LaserFan3D fan = ToLaserFan3D(laser_scan, 0.f, 10.f, 1.f);
EXPECT_TRUE(fan.point_cloud[0].isApprox(Eigen::Vector2f(1.f, 0.f), 1e-6)); EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(fan.point_cloud[1].isApprox( EXPECT_TRUE(fan.returns[1].isApprox(
Eigen::Vector2f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6)); Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
EXPECT_TRUE(fan.point_cloud[2].isApprox(Eigen::Vector2f(0.f, 1.f), 1e-6)); EXPECT_TRUE(fan.returns[2].isApprox(Eigen::Vector3f(0.f, 1.f, 0.f), 1e-6));
EXPECT_TRUE(fan.point_cloud[3].isApprox( EXPECT_TRUE(fan.returns[3].isApprox(
Eigen::Vector2f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6)); Eigen::Vector3f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
EXPECT_TRUE(fan.point_cloud[4].isApprox(Eigen::Vector2f(-1.f, 0.f), 1e-6)); EXPECT_TRUE(fan.returns[4].isApprox(Eigen::Vector3f(-1.f, 0.f, 0.f), 1e-6));
EXPECT_TRUE(fan.point_cloud[5].isApprox( EXPECT_TRUE(fan.returns[5].isApprox(
Eigen::Vector2f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f)), 1e-6)); Eigen::Vector3f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f),
EXPECT_TRUE(fan.point_cloud[6].isApprox(Eigen::Vector2f(0.f, -1.f), 1e-6)); 1e-6));
EXPECT_TRUE(fan.point_cloud[7].isApprox( EXPECT_TRUE(fan.returns[6].isApprox(Eigen::Vector3f(0.f, -1.f, 0.f), 1e-6));
Eigen::Vector2f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f)), 1e-6)); EXPECT_TRUE(fan.returns[7].isApprox(
Eigen::Vector3f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f), 0.f), 1e-6));
} }
TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) { TEST(ProjectorTest, ToLaserFan3DWithInfinityAndNaN) {
proto::LaserScan laser_scan; proto::LaserScan laser_scan;
laser_scan.add_range()->add_value(1.f); laser_scan.add_range()->add_value(1.f);
laser_scan.add_range()->add_value(std::numeric_limits<float>::infinity()); laser_scan.add_range()->add_value(std::numeric_limits<float>::infinity());
@ -63,14 +64,14 @@ TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) {
laser_scan.set_angle_max(3.f * static_cast<float>(M_PI_4)); 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_angle_increment(static_cast<float>(M_PI_4));
const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f); const LaserFan3D fan = ToLaserFan3D(laser_scan, 2.f, 10.f, 1.f);
ASSERT_EQ(2, fan.point_cloud.size()); ASSERT_EQ(2, fan.returns.size());
EXPECT_TRUE(fan.point_cloud[0].isApprox(Eigen::Vector2f(0.f, 2.f), 1e-6)); EXPECT_TRUE(fan.returns[0].isApprox(Eigen::Vector3f(0.f, 2.f, 0.f), 1e-6));
EXPECT_TRUE(fan.point_cloud[1].isApprox(Eigen::Vector2f(-3.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.missing_echo_point_cloud.size()); ASSERT_EQ(1, fan.misses.size());
EXPECT_TRUE(fan.missing_echo_point_cloud[0].isApprox( EXPECT_TRUE(fan.misses[0].isApprox(
Eigen::Vector2f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6)); Eigen::Vector3f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f), 0.f), 1e-6));
} }
// Custom matcher for pair<eigen::Vector3f, int> entries. // Custom matcher for pair<eigen::Vector3f, int> entries.