Surface normal LiDAR sensor

Docs/ref_sensors.md

@@ -18,6 +18,7 @@
 - [__V2X sensor__](#v2x-sensor)
     - [Cooperative awareness](#cooperative-awareness-message)
     - [Custom message](#custom-v2x-message)
+- [__Surface normal LiDAR sensor__](#surface-normal-lidar-sensor)
 
 !!! Important
     All the sensors use the UE coordinate system (__x__-*forward*, __y__-*right*, __z__-*up*), and return coordinates in local space. When using any visualization software, pay attention to its coordinate system. Many invert the Y-axis, so visualizing the sensor data directly may result in mirrored outputs.  
@@ -1004,3 +1005,15 @@ Example:
 | path\_loss\_model       | string | geometric     |   general path loss model to be used. Options: [geometric, winner]  |
 | use\_etsi\_fading       | bool   | true         |   Use the fading params as mentioned in the ETSI publication (true), or use the custom fading standard deviation         |
 | custom\_fading\_stddev  | float  | 0.0      |   Custom value for fading standard deviation, only used if `use_etsi_fading` is set to `false`              |
+
+<br>
+
+---
+## Surface normal LIDAR sensor
+
+* __Blueprint:__ sensor.lidar.ray_cast_surface_normals
+* __Output:__ [carla.SurfaceNormalLidarMeasurement](python_api.md#carla.SurfaceNormalLidarMeasurement) per step (unless `sensor_tick` says otherwise).
+
+This sensor shares the same interface as the [LIDAR sensor](#lidar-sensor). In addition to providing the `x, y, z` coordinates and intensity value `i`, surface normal LiDAR sensors also return a globally oriented surface normal vector `nx, ny, nz`. This vector represents the global orientation of the surface at the point where the laser ray intersects it.
+
+![SurfaceNormalLidarPointCloud](img/surface_normal_lidar_point_cloud.png)

LibCarla/source/carla/sensor/SensorRegistry.h

@@ -27,6 +27,7 @@
 #include "carla/sensor/s11n/ObstacleDetectionEventSerializer.h"
 #include "carla/sensor/s11n/RadarSerializer.h"
 #include "carla/sensor/s11n/SemanticLidarSerializer.h"
+#include "carla/sensor/s11n/SurfaceNormalLidarSerializer.h"
 #include "carla/sensor/s11n/GBufferUint8Serializer.h"
 #include "carla/sensor/s11n/GBufferFloatSerializer.h"
 #include "carla/sensor/s11n/V2XSerializer.h"
@@ -44,6 +45,7 @@ class AOpticalFlowCamera;
 class ARadar;
 class ARayCastSemanticLidar;
 class ARayCastLidar;
+class ARayCastSurfaceNormalLidar;
 class ASceneCaptureCamera;
 class ASemanticSegmentationCamera;
 class AInstanceSegmentationCamera;
@@ -77,6 +79,7 @@ namespace sensor {
     std::pair<ARadar *, s11n::RadarSerializer>,
     std::pair<ARayCastSemanticLidar *, s11n::SemanticLidarSerializer>,
     std::pair<ARayCastLidar *, s11n::LidarSerializer>,
+    std::pair<ARayCastSurfaceNormalLidar *, s11n::SurfaceNormalLidarSerializer>,
     std::pair<ARssSensor *, s11n::NoopSerializer>,
     std::pair<ASceneCaptureCamera *, s11n::ImageSerializer>,
     std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
@@ -110,6 +113,7 @@ namespace sensor {
 #include "Carla/Sensor/Radar.h"
 #include "Carla/Sensor/RayCastLidar.h"
 #include "Carla/Sensor/RayCastSemanticLidar.h"
+#include "Carla/Sensor/RayCastSurfaceNormalLidar.h"
 #include "Carla/Sensor/RssSensor.h"
 #include "Carla/Sensor/SceneCaptureCamera.h"
 #include "Carla/Sensor/SemanticSegmentationCamera.h"

LibCarla/source/carla/sensor/data/SurfaceNormalLidarData.h

@@ -0,0 +1,135 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/rpc/Location.h"
+#include "carla/sensor/data/SemanticLidarData.h"
+#include "carla/sensor/data/LidarData.h"
+
+#include <cstdint>
+#include <vector>
+
+namespace carla {
+
+namespace ros2 {
+  class ROS2;
+}
+
+namespace sensor {
+
+namespace s11n {
+  class SurfaceNormalLidarSerializer;
+  class SurfaceNormalLidarHeaderView;
+}
+
+namespace data {
+
+  /// Helper class to store and serialize the data generated by a Lidar.
+  ///
+  /// The header of a Lidar measurement consists of an array of uint32_t's in
+  /// the following layout
+  ///
+  ///    {
+  ///      Horizontal angle (float),
+  ///      Channel count,
+  ///      Point count of channel 0,
+  ///      ...
+  ///      Point count of channel n,
+  ///    }
+  ///
+  /// The points are stored in an array of floats
+  ///
+  ///    {
+  ///      X0, Y0, Z0, I0, NX0, NY0, NZ0
+  ///      ...
+  ///      Xn, Yn, Zn, In, NXn, NYn, NZn
+  ///    }
+  ///
+
+  class SurfaceNormalLidarDetection {
+    public:
+      geom::Location point;
+      float intensity;
+      geom::Vector3D surf_normal;
+
+      SurfaceNormalLidarDetection() :
+          point(0.0f, 0.0f, 0.0f), intensity{0.0f}, surf_normal(0.0f, 0.0f, 0.0f) { }
+      SurfaceNormalLidarDetection(float x, float y, float z, float intensity, float nx, float ny, float nz) :
+          point(x, y, z), intensity{intensity}, surf_normal(nx, ny, nz) { }
+      SurfaceNormalLidarDetection(geom::Location p, float intensity, geom::Location surf_normal) :
+          point(p), intensity{intensity}, surf_normal(surf_normal) { }
+      void WritePlyHeaderInfo(std::ostream& out) const{
+        out << "property float32 x\n" \
+          "property float32 y\n" \
+          "property float32 z\n" \
+          "property float32 I\n" \
+          "property float32 nx\n" \
+          "property float32 ny\n" \
+          "property float32 nz";
+      }
+
+      void WriteDetection(std::ostream& out) const{
+        out << point.x << ' ' << point.y << ' ' << point.z << ' ' << intensity
+                << ' ' << surf_normal.x << ' ' << surf_normal.y << ' ' << surf_normal.z;
+      }
+  };
+
+  class SurfaceNormalLidarData : public SemanticLidarData{
+
+
+  public: 
+    explicit SurfaceNormalLidarData(uint32_t ChannelCount = 0u)
+      : SemanticLidarData(ChannelCount) {
+    }
+
+    SurfaceNormalLidarData &operator=(SurfaceNormalLidarData &&) = default;
+
+    ~SurfaceNormalLidarData() = default;
+
+    virtual void ResetMemory(std::vector<uint32_t> points_per_channel) {
+      DEBUG_ASSERT(GetChannelCount() > points_per_channel.size());
+      std::memset(_header.data() + Index::SIZE, 0, sizeof(uint32_t) * GetChannelCount());
+
+      uint32_t total_points = static_cast<uint32_t>(
+          std::accumulate(points_per_channel.begin(), points_per_channel.end(), 0));
+
+      _points.clear();
+      _points.reserve(total_points * 7);
+    }
+
+    void WritePointSync(SurfaceNormalLidarDetection &detection) {
+      _points.emplace_back(detection.point.x);
+      _points.emplace_back(detection.point.y);
+      _points.emplace_back(detection.point.z);
+      _points.emplace_back(detection.intensity);
+      _points.emplace_back(detection.surf_normal.x);
+      _points.emplace_back(detection.surf_normal.y);
+      _points.emplace_back(detection.surf_normal.z);
+    }
+
+    virtual void WritePointSync(LidarDetection &detection) {
+      (void) detection;
+      DEBUG_ASSERT(false);
+    }
+
+    virtual void WritePointSync(SemanticLidarDetection &detection) {
+      (void) detection;
+      DEBUG_ASSERT(false);
+    }
+
+  private:
+    std::vector<float> _points;
+
+    friend class s11n::SurfaceNormalLidarSerializer;
+    friend class s11n::SurfaceNormalLidarHeaderView;
+    friend class carla::ros2::ROS2;
+  };
+
+} // namespace s11n
+} // namespace sensor
+} // namespace carla
+

LibCarla/source/carla/sensor/data/SurfaceNormalLidarMeasurement.h

@@ -0,0 +1,61 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/Debug.h"
+#include "carla/rpc/Location.h"
+#include "carla/sensor/data/Array.h"
+#include "carla/sensor/s11n/SurfaceNormalLidarSerializer.h"
+
+namespace carla {
+namespace sensor {
+namespace data {
+
+  /// Measurement produced by a Lidar. Consists of an array of 3D points plus
+  /// some extra meta-information about the Lidar.
+  class SurfaceNormalLidarMeasurement : public Array<data::SurfaceNormalLidarDetection>  {
+    static_assert(sizeof(data::SurfaceNormalLidarDetection) == 7u * sizeof(float), "Location size missmatch");
+    using Super = Array<data::SurfaceNormalLidarDetection>;
+
+  protected:
+    using Serializer = s11n::SurfaceNormalLidarSerializer;
+
+    friend Serializer;
+
+    explicit SurfaceNormalLidarMeasurement(RawData &&data)
+      : Super(std::move(data), [](const RawData &d) {
+      return Serializer::GetHeaderOffset(d);
+    }) {}
+
+  private:
+
+    auto GetHeader() const {
+      return Serializer::DeserializeHeader(Super::GetRawData());
+    }
+
+  public:
+    /// Horizontal angle of the Lidar at the time of the measurement.
+    auto GetHorizontalAngle() const {
+      return GetHeader().GetHorizontalAngle();
+    }
+
+    /// Number of channels of the Lidar.
+    auto GetChannelCount() const {
+      return GetHeader().GetChannelCount();
+    }
+
+    /// Retrieve the number of points that @a channel generated. Points are
+    /// sorted by channel, so this method allows to identify the channel that
+    /// generated each point.
+    auto GetPointCount(size_t channel) const {
+      return GetHeader().GetPointCount(channel);
+    }
+  };
+
+} // namespace data
+} // namespace sensor
+} // namespace carla

LibCarla/source/carla/sensor/s11n/SurfaceNormalLidarSerializer.cpp

@@ -0,0 +1,22 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+
+#include "carla/sensor/data/SurfaceNormalLidarMeasurement.h"
+#include "carla/sensor/s11n/SurfaceNormalLidarSerializer.h"
+
+namespace carla {
+namespace sensor {
+namespace s11n {
+
+  SharedPtr<SensorData> SurfaceNormalLidarSerializer::Deserialize(RawData &&data) {
+    return SharedPtr<data::SurfaceNormalLidarMeasurement>(
+        new data::SurfaceNormalLidarMeasurement{std::move(data)});
+  }
+
+} // namespace s11n
+} // namespace sensor
+} // namespace carla

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

@@ -0,0 +1,97 @@
+// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/Debug.h"
+#include "carla/Memory.h"
+#include "carla/sensor/RawData.h"
+#include "carla/sensor/data/SurfaceNormalLidarData.h"
+
+namespace carla {
+namespace sensor {
+
+  class SensorData;
+
+namespace s11n {
+
+  // ===========================================================================
+  // -- SurfaceNormalLidarHeaderView --------------------------------------------------------
+  // ===========================================================================
+
+  /// A view over the header of a SurfaceNormalLidar measurement.
+  class SurfaceNormalLidarHeaderView {
+    using Index = data::SurfaceNormalLidarData::Index;
+
+  public:
+    float GetHorizontalAngle() const {
+      return reinterpret_cast<const float &>(_begin[Index::HorizontalAngle]);
+    }
+
+    uint32_t GetChannelCount() const {
+      return _begin[Index::ChannelCount];
+    }
+
+    uint32_t GetPointCount(size_t channel) const {
+      DEBUG_ASSERT(channel < GetChannelCount());
+      return _begin[Index::SIZE + channel];
+    }
+
+  private:
+    friend class SurfaceNormalLidarSerializer;
+
+    explicit SurfaceNormalLidarHeaderView(const uint32_t *begin) : _begin(begin) {
+      DEBUG_ASSERT(_begin != nullptr);
+    }
+
+    const uint32_t *_begin;
+  };
+
+  // ===========================================================================
+  // -- SurfaceNormalLidarSerializer --------------------------------------------------------
+  // ===========================================================================
+
+  /// Serializes the data generated by Lidar sensors.
+  class SurfaceNormalLidarSerializer {
+  public:
+
+    static SurfaceNormalLidarHeaderView DeserializeHeader(const RawData &data) {
+      return SurfaceNormalLidarHeaderView{reinterpret_cast<const uint32_t *>(data.begin())};
+    }
+
+    static size_t GetHeaderOffset(const RawData &data) {
+      auto View = DeserializeHeader(data);
+      return sizeof(uint32_t) * (View.GetChannelCount() + data::SurfaceNormalLidarData::Index::SIZE);
+    }
+
+    template <typename Sensor>
+    static Buffer Serialize(
+        const Sensor &sensor,
+        const data::SurfaceNormalLidarData &data,
+        Buffer &&output);
+
+    static SharedPtr<SensorData> Deserialize(RawData &&data);
+  };
+
+  // ===========================================================================
+  // -- SurfaceNormalLidarSerializer implementation -----------------------------------------
+  // ===========================================================================
+
+  template <typename Sensor>
+  inline Buffer SurfaceNormalLidarSerializer::Serialize(
+      const Sensor &,
+      const data::SurfaceNormalLidarData &data,
+      Buffer &&output) {
+    std::array<boost::asio::const_buffer, 2u> seq = {
+        boost::asio::buffer(data._header),
+        boost::asio::buffer(data._points)};
+    output.copy_from(seq);
+    return std::move(output);
+  }
+
+} // namespace s11n
+} // namespace sensor
+} // namespace carla