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

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 *
+        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();

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

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