From 628b9da6d226ba9825f3bbf941fc8f4b98016a0f Mon Sep 17 00:00:00 2001
From: Holger Rapp <HolgerRapp@gmx.net>
Date: Tue, 20 Dec 2016 16:20:00 +0100
Subject: [PATCH] Extract intensities for 2D lasers on conversion. (#167)

---
 cartographer/sensor/laser.cc      | 19 +++++++++++++++----
 cartographer/sensor/laser.h       |  7 +++++++
 cartographer/sensor/point_cloud.h |  5 +++++
 3 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/cartographer/sensor/laser.cc b/cartographer/sensor/laser.cc
index d5ce47f..e453ef6 100644
--- a/cartographer/sensor/laser.cc
+++ b/cartographer/sensor/laser.cc
@@ -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 *
-                              (first_echo * Eigen::Vector3f::UnitX()));
+        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();
diff --git a/cartographer/sensor/laser.h b/cartographer/sensor/laser.h
index 3e838d4..882ffe9 100644
--- a/cartographer/sensor/laser.h
+++ b/cartographer/sensor/laser.h
@@ -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);
 
diff --git a/cartographer/sensor/point_cloud.h b/cartographer/sensor/point_cloud.h
index 87479e3..b163278 100644
--- a/cartographer/sensor/point_cloud.h
+++ b/cartographer/sensor/point_cloud.h
@@ -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);