Lidar Intensity: Added intensity to lidar's output

For now, only takes into account the intensity loss
due to the the atmosphere atenuation.

LibCarla/source/carla/sensor/s11n/LidarMeasurement.h

@@ -31,14 +31,30 @@ namespace s11n {
   /// The points are stored in an array of floats
   ///
   ///    {
-  ///      X0, Y0, Z0,
+  ///      X0, Y0, Z0, I0
   ///      ...
-  ///      Xn, Yn, Zn,
+  ///      Xn, Yn, Zn, In
   ///    }
   ///
   /// @warning WritePoint should be called sequentially in the order in which
   /// the points are going to be stored, i.e., starting at channel zero and
   /// increasing steadily.
+
+  class LidarDetection {
+    public:
+      float x; // m/s
+      float y;  // rad
+      float z; // rad
+      float intensity;    // m
+      static const int SIZE = 4;
+
+      LidarDetection(float x, float y, float z, float intensity) :
+          x{x*1e-2f}, y{y*1e-2f}, z{z*1e-2f}, intensity{intensity} { }
+      LidarDetection(rpc::Location p, float intensity) :
+          x{p.x}, y{p.y}, z{p.z}, intensity{intensity} { }
+  };
+
+
   class LidarMeasurement {
     static_assert(sizeof(float) == sizeof(uint32_t), "Invalid float size");
 
@@ -72,25 +88,46 @@ namespace s11n {
       return _header[Index::ChannelCount];
     }
 
-    void Reset(uint32_t total_point_count) {
+    void Reset(uint32_t channels, uint32_t channel_point_count) {
       std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
       _points.clear();
-      _points.reserve(3u * total_point_count);
+      _points.reserve(LidarDetection::SIZE * channels * channel_point_count);
+
+      _aux_points.resize(channels);
+
+      for (auto& aux : _aux_points) {
+        aux.clear();
+        aux.reserve(channel_point_count);
+      }
     }
 
-    void WritePoint(uint32_t channel, rpc::Location point) {
+    void WritePointAsync(uint32_t channel, LidarDetection detection) {
       DEBUG_ASSERT(GetChannelCount() > channel);
-      _header[Index::SIZE + channel] += 1u;
-      _points.emplace_back(point.x);
-      _points.emplace_back(point.y);
-      _points.emplace_back(point.z);
+      _aux_points[channel].emplace_back(detection);
+    }
+
+    void SaveDetections() {
+      _points.clear();
+
+      for (auto idxChannel = 0u; idxChannel < GetChannelCount(); ++idxChannel) {
+        _header[Index::SIZE + idxChannel] = _aux_points.size();
+        for (auto& Pt : _aux_points[idxChannel]) {
+          _points.emplace_back(Pt.x);
+          _points.emplace_back(Pt.y);
+          _points.emplace_back(Pt.z);
+          _points.emplace_back(Pt.intensity);
+        }
+      }
+
     }
 
   private:
 
     std::vector<uint32_t> _header;
+    std::vector<std::vector<LidarDetection>> _aux_points;
 
     std::vector<float> _points;
+
   };
 
 } // namespace s11n

PythonAPI/examples/manual_control.py

@@ -979,7 +979,7 @@ class CameraManager(object):
             return
         if self.sensors[self.index][0].startswith('sensor.lidar'):
             points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
-            points = np.reshape(points, (int(points.shape[0] / 3), 3))
+            points = np.reshape(points, (int(points.shape[0] / 4), 4))
             lidar_data = np.array(points[:, :2])
             lidar_data *= min(self.hud.dim) / (2.0 * self.lidar_range)
             lidar_data += (0.5 * self.hud.dim[0], 0.5 * self.hud.dim[1])

PythonAPI/util/raycast_sensor_testing.py

@@ -188,7 +188,7 @@ class SensorManager:
         lidar_range = 2.0*float(self.sensor_options['range'])
 
         points = np.frombuffer(image.raw_data, dtype=np.dtype('f4'))
-        points = np.reshape(points, (int(points.shape[0] / 3), 3))
+        points = np.reshape(points, (int(points.shape[0] / 4), 4))
         lidar_data = np.array(points[:, :2])
         lidar_data *= min(disp_size) / lidar_range
         lidar_data += (0.5 * disp_size[0], 0.5 * disp_size[1])

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.cpp

@@ -95,40 +95,49 @@ void ARayCastLidar::ReadPoints(const float DeltaTime)
   const float AngleDistanceOfTick = Description.RotationFrequency * 360.0f * DeltaTime;
   const float AngleDistanceOfLaserMeasure = AngleDistanceOfTick / PointsToScanWithOneLaser;
 
-  LidarMeasurement.Reset(ChannelCount * PointsToScanWithOneLaser);
-  AuxPoints.resize(ChannelCount);
+  LidarMeasurement.Reset(ChannelCount,  PointsToScanWithOneLaser);
 
 
   GetWorld()->GetPhysicsScene()->GetPxScene()->lockRead();
   ParallelFor(ChannelCount, [&](int32 idxChannel) {
-    AuxPoints[idxChannel].clear();
-    AuxPoints[idxChannel].reserve(PointsToScanWithOneLaser);
 
     FCriticalSection Mutex;
     ParallelFor(PointsToScanWithOneLaser, [&](int32 idxPtsOneLaser) {
       FVector Point;
+      float Intensity;
       const float Angle = CurrentHorizontalAngle + AngleDistanceOfLaserMeasure * idxPtsOneLaser;
-      if (ShootLaser(idxChannel, Angle, Point)) {
+      if (ShootLaser(idxChannel, Angle, Point, Intensity)) {
         Mutex.Lock();
-        AuxPoints[idxChannel].emplace_back(Point);
+        LidarMeasurement.WritePointAsync(idxChannel, {Point, Intensity});
         Mutex.Unlock();
       }
     });
   });
   GetWorld()->GetPhysicsScene()->GetPxScene()->unlockRead();
 
-  for (auto idxChannel = 0u; idxChannel < ChannelCount; ++idxChannel) {
-    for (auto& Pt : AuxPoints[idxChannel]) {
-      LidarMeasurement.WritePoint(idxChannel, Pt);
-    }
-  }
+  LidarMeasurement.SaveDetections();
+
 
   const float HorizontalAngle = carla::geom::Math::ToRadians(
       std::fmod(CurrentHorizontalAngle + AngleDistanceOfTick, 360.0f));
   LidarMeasurement.SetHorizontalAngle(HorizontalAngle);
 }
 
-bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ) const
+float ARayCastLidar::ComputeIntensity(const FVector &LidarBodyLoc, const FVector &XYZ, const FHitResult& HitInfo) const
+{
+  const float Distance = XYZ.Size();
+  const float AttenAtm = -0.004;
+
+  const float IntEm = 1.0f;
+
+  const float AbsAtm = exp(AttenAtm * Distance);
+
+  const float IntRec = IntEm * AbsAtm;
+
+  return IntRec;
+}
+
+bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle, FVector &XYZ, float &Intensity) const
 {
   const float VerticalAngle = LaserAngles[Channel];
 
@@ -175,6 +184,9 @@ bool ARayCastLidar::ShootLaser(const uint32 Channel, const float HorizontalAngle
     const FVector hp = HitInfo.ImpactPoint;
     XYZ = actorTransf.Inverse().TransformPosition(hp);
 
+    Intensity = ComputeIntensity(LidarBodyLoc, XYZ, HitInfo);
+
+
     return true;
   } else {
     return false;

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastLidar.h

@@ -48,13 +48,15 @@ private:
   void ReadPoints(float DeltaTime);
 
   /// Shoot a laser ray-trace, return whether the laser hit something.
-  bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point) const;
+  bool ShootLaser(uint32 Channel, float HorizontalAngle, FVector &Point, float& Intensity) const;
+
+  /// Compute the received intensity of the point
+  float ComputeIntensity(const FVector &LidarBodyLoc, const FVector &XYZ, const FHitResult& HitInfo) const;
 
   UPROPERTY(EditAnywhere)
   FLidarDescription Description;
 
   TArray<float> LaserAngles;
-  std::vector<std::vector<FVector>> AuxPoints;
 
   FLidarMeasurement LidarMeasurement;
 };