Extract intensities for 2D lasers on conversion. (#167)

master
Holger Rapp 2016-12-20 16:20:00 +01:00 committed by GitHub
parent 4e9c3d69b5
commit 628b9da6d2
3 changed files with 27 additions and 4 deletions

View File

@ -38,6 +38,11 @@ std::vector<uint8> ReorderReflectivities(
} // namespace
PointCloud ToPointCloud(const proto::LaserScan& proto) {
return ToPointCloudWithIntensities(proto).points;
}
PointCloudWithIntensities ToPointCloudWithIntensities(
const proto::LaserScan& proto) {
CHECK_GE(proto.range_min(), 0.f);
CHECK_GE(proto.range_max(), proto.range_min());
if (proto.angle_increment() > 0.f) {
@ -45,15 +50,21 @@ PointCloud ToPointCloud(const proto::LaserScan& proto) {
} else {
CHECK_GT(proto.angle_min(), proto.angle_max());
}
PointCloud point_cloud;
PointCloudWithIntensities point_cloud;
float angle = proto.angle_min();
for (const auto& range : proto.range()) {
for (int i = 0; i < proto.range_size(); ++i) {
const auto& range = proto.range(i);
if (range.value_size() > 0) {
const float first_echo = range.value(0);
if (proto.range_min() <= first_echo && first_echo <= proto.range_max()) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
point_cloud.push_back(rotation *
point_cloud.points.push_back(rotation *
(first_echo * Eigen::Vector3f::UnitX()));
if (proto.intensity_size() > 0) {
point_cloud.intensities.push_back(proto.intensity(i).value(0));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
angle += proto.angle_increment();

View File

@ -42,6 +42,13 @@ struct LaserFan {
// outside the valid range described by 'proto'.
PointCloud ToPointCloud(const proto::LaserScan& proto);
// Like above, but also extracts intensities of ouf the laser scan. The
// intensities of the laser are device specific and therefore require
// normalization to be comparable. In case the 'proto' does not contain
// intensities, this will return all 0. for the intensities.
PointCloudWithIntensities ToPointCloudWithIntensities(
const proto::LaserScan& proto);
// Converts 'laser_fan' to a proto::LaserFan.
proto::LaserFan ToProto(const LaserFan& laser_fan);

View File

@ -30,6 +30,11 @@ namespace sensor {
typedef std::vector<Eigen::Vector3f> PointCloud;
struct PointCloudWithIntensities {
PointCloud points;
std::vector<float> intensities;
};
// Transforms 'point_cloud' according to 'transform'.
PointCloud TransformPointCloud(const PointCloud& point_cloud,
const transform::Rigid3f& transform);