Skip to content

Commit

Permalink
Simplify the conversion of LaserScan to LaserFan3D. (#57)
Browse files Browse the repository at this point in the history
  • Loading branch information
@wohe
wohe committed Oct 13, 2016
1 parent 5188509 commit 7d93ac3
Show file tree
Hide file tree
Showing 3 changed files with 98 additions and 105 deletions.
113 changes: 53 additions & 60 deletions cartographer/sensor/laser.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,25 +37,33 @@ 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) {
LaserFan TransformLaserFan(const LaserFan& laser_fan,
const transform::Rigid2f& transform) {
return LaserFan{
transform * laser_fan.origin,
TransformPointCloud2D(laser_fan.point_cloud, transform),
TransformPointCloud2D(laser_fan.missing_echo_point_cloud, transform)};
}

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());
LaserFan laser_fan = {Eigen::Vector2f::Zero(), {}, {}};
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.point_cloud.push_back(Eigen::Rotation2Df(angle) *
Eigen::Vector2f(first_echo, 0.f));
laser_fan.returns.push_back(rotation *
(first_echo * Eigen::Vector3f::UnitX()));
} else {
laser_fan.missing_echo_point_cloud.push_back(
Eigen::Rotation2Df(angle) *
Eigen::Vector2f(missing_echo_ray_length, 0.f));
laser_fan.misses.push_back(
rotation * (missing_echo_ray_length * Eigen::Vector3f::UnitX()));
}
}
}
Expand All @@ -64,57 +72,6 @@ LaserFan ToLaserFan(const proto::LaserScan& proto, const float min_range,
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{
transform * laser_fan.origin,
TransformPointCloud2D(laser_fan.point_cloud, transform),
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<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,
};
}

proto::LaserFan3D ToProto(const LaserFan3D& laser_fan) {
proto::LaserFan3D proto;
*proto.mutable_origin() = transform::ToProto(laser_fan.origin);
Expand All @@ -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, {}, {}, {}};
Expand All @@ -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
45 changes: 22 additions & 23 deletions cartographer/sensor/laser.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -54,43 +52,44 @@ struct LaserFan3D {
std::vector<uint8> reflectivities;
};

// Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D.
struct CompressedLaserFan3D {
Eigen::Vector3f origin;
CompressedPointCloud returns;
CompressedPointCloud misses;

// Reflectivity value of returns.
std::vector<uint8> reflectivities;
};

LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);

CompressedLaserFan3D Compress(const LaserFan3D& laser_fan);
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 ToLaserFan3D(const LaserFan& laser_fan);

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);

// 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);
// Like LaserFan3D but with compressed point clouds. The point order changes
// when converting from LaserFan3D.
struct CompressedLaserFan3D {
Eigen::Vector3f origin;
CompressedPointCloud returns;
CompressedPointCloud misses;

// Reflectivity value of returns.
std::vector<uint8> reflectivities;
};

CompressedLaserFan3D Compress(const LaserFan3D& laser_fan);

LaserFan3D Decompress(const CompressedLaserFan3D& compressed_laser_fan);

} // namespace sensor
} // namespace cartographer
Expand Down
45 changes: 23 additions & 22 deletions cartographer/sensor/laser_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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());
Expand All @@ -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.
Expand Down

0 comments on commit 7d93ac3

Please sign in to comment.