Simplify the conversion of LaserScan to LaserFan3D. (#57)
parent
518850999f
commit
7d93ac3302
|
@ -37,41 +37,6 @@ 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);
|
||||
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,
|
||||
const transform::Rigid2f& transform) {
|
||||
return LaserFan{
|
||||
|
@ -80,39 +45,31 @@ LaserFan TransformLaserFan(const LaserFan& laser_fan,
|
|||
TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)};
|
||||
}
|
||||
|
||||
LaserFan3D ToLaserFan3D(const LaserFan& laser_fan) {
|
||||
return LaserFan3D{
|
||||
Eigen::Vector3f(laser_fan.origin.x(), laser_fan.origin.y(), 0.),
|
||||
ToPointCloud(laser_fan.point_cloud),
|
||||
ToPointCloud(laser_fan.missing_echo_point_cloud)};
|
||||
LaserFan3D ToLaserFan3D(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());
|
||||
LaserFan3D laser_fan = {Eigen::Vector3f::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) {
|
||||
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
|
||||
if (first_echo <= max_range) {
|
||||
laser_fan.returns.push_back(rotation *
|
||||
(first_echo * Eigen::Vector3f::UnitX()));
|
||||
} else {
|
||||
laser_fan.misses.push_back(
|
||||
rotation * (missing_echo_ray_length * Eigen::Vector3f::UnitX()));
|
||||
}
|
||||
|
||||
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};
|
||||
}
|
||||
|
||||
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 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,
|
||||
};
|
||||
angle += proto.angle_increment();
|
||||
}
|
||||
return laser_fan;
|
||||
}
|
||||
|
||||
proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) {
|
||||
|
@ -135,6 +92,16 @@ LaserFan3D FromProto(const proto::LaserFan3D& proto) {
|
|||
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,
|
||||
const float max_range) {
|
||||
LaserFan3D result{laser_fan.origin, {}, {}, {}};
|
||||
|
@ -146,5 +113,31 @@ LaserFan3D FilterLaserFanByMaxRange(const LaserFan3D& laser_fan,
|
|||
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 cartographer
|
||||
|
|
|
@ -34,8 +34,6 @@ struct LaserFan {
|
|||
PointCloud2D 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'.
|
||||
LaserFan TransformLaserFan(const LaserFan& laser_fan,
|
||||
|
@ -54,6 +52,30 @@ struct LaserFan3D {
|
|||
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
|
||||
// when converting from LaserFan3D.
|
||||
struct CompressedLaserFan3D {
|
||||
|
@ -65,32 +87,9 @@ struct CompressedLaserFan3D {
|
|||
std::vector<uint8> reflectivities;
|
||||
};
|
||||
|
||||
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);
|
||||
|
||||
CompressedLaserFan3D Compress(const LaserFan3D& laser_fan);
|
||||
|
||||
// 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 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);
|
||||
LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);
|
||||
|
||||
} // namespace sensor
|
||||
} // namespace cartographer
|
||||
|
|
|
@ -28,7 +28,7 @@ namespace {
|
|||
using ::testing::Contains;
|
||||
using ::testing::PrintToString;
|
||||
|
||||
TEST(ProjectorTest, ToLaserFan) {
|
||||
TEST(ProjectorTest, ToLaserFan3D) {
|
||||
proto::LaserScan laser_scan;
|
||||
for (int i = 0; i < 8; ++i) {
|
||||
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_increment(static_cast<float>(M_PI_4));
|
||||
|
||||
const LaserFan fan = ToLaserFan(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.point_cloud[1].isApprox(
|
||||
Eigen::Vector2f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[2].isApprox(Eigen::Vector2f(0.f, 1.f), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[3].isApprox(
|
||||
Eigen::Vector2f(-1.f / std::sqrt(2.f), 1.f / std::sqrt(2.f)), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[4].isApprox(Eigen::Vector2f(-1.f, 0.f), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[5].isApprox(
|
||||
Eigen::Vector2f(-1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f)), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[6].isApprox(Eigen::Vector2f(0.f, -1.f), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[7].isApprox(
|
||||
Eigen::Vector2f(1.f / std::sqrt(2.f), -1.f / std::sqrt(2.f)), 1e-6));
|
||||
const LaserFan3D fan = ToLaserFan3D(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(
|
||||
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(
|
||||
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(
|
||||
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(
|
||||
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;
|
||||
laser_scan.add_range()->add_value(1.f);
|
||||
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_increment(static_cast<float>(M_PI_4));
|
||||
|
||||
const LaserFan fan = ToLaserFan(laser_scan, 2.f, 10.f, 1.f);
|
||||
ASSERT_EQ(2, fan.point_cloud.size());
|
||||
EXPECT_TRUE(fan.point_cloud[0].isApprox(Eigen::Vector2f(0.f, 2.f), 1e-6));
|
||||
EXPECT_TRUE(fan.point_cloud[1].isApprox(Eigen::Vector2f(-3.f, 0.f), 1e-6));
|
||||
const LaserFan3D fan = ToLaserFan3D(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.missing_echo_point_cloud.size());
|
||||
EXPECT_TRUE(fan.missing_echo_point_cloud[0].isApprox(
|
||||
Eigen::Vector2f(1.f / std::sqrt(2.f), 1.f / std::sqrt(2.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));
|
||||
}
|
||||
|
||||
// Custom matcher for pair<eigen::Vector3f, int> entries.
|
||||
|
|
Loading…
Reference in New Issue