From 7d93ac3302defcdde77d75d2bc7c38ca99eeae08 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Thu, 13 Oct 2016 15:09:36 +0200 Subject: [PATCH] Simplify the conversion of LaserScan to LaserFan3D. (#57) --- cartographer/sensor/laser.cc | 129 ++++++++++++++---------------- cartographer/sensor/laser.h | 51 ++++++------ cartographer/sensor/laser_test.cc | 45 ++++++----- 3 files changed, 109 insertions(+), 116 deletions(-) diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc index 400e501..24ced7e 100644 --- a/cartographer/sensor/laser.cc +++ b/cartographer/sensor/laser.cc @@ -37,41 +37,6 @@ std::vector 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 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 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, - }; +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())); + } + } + } + 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 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 diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h index 453ed39..40efd69 100644 --- a/cartographer/sensor/laser.h +++ b/cartographer/sensor/laser.h @@ -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 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 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 diff --git a/cartographer/sensor/laser_test.cc b/cartographer/sensor/laser_test.cc index 6bf210c..d031a05 100644 --- a/cartographer/sensor/laser_test.cc +++ b/cartographer/sensor/laser_test.cc @@ -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(M_PI_4)); laser_scan.set_angle_increment(static_cast(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::infinity()); @@ -63,14 +64,14 @@ TEST(ProjectorTest, ToLaserFanWithInfinityAndNaN) { laser_scan.set_angle_max(3.f * static_cast(M_PI_4)); laser_scan.set_angle_increment(static_cast(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 entries.