Implement an Optical Flow Camera

CHANGELOG.md

@@ -29,6 +29,7 @@
   * Exposed tire parameters for longitudinal and lateral stiffness in the PhysicsControl. 
   * When setting a global plan at the LocalPlanner, it is now optional to stop the automatic fill of the waypoint buffer
   * Improved agent's vehicle detection to also take into account the actor bounding boxes
+  * Added Optical Flow camera
   * API extensions:
   - Added `set_wheel_steer_direction()` function to change the bone angle of each wheel of a vehicle
   - Added `get_wheel_steer_angle()` function to get the steer angle of a vehicle whee

LibCarla/source/carla/sensor/SensorRegistry.h

@@ -19,6 +19,7 @@
 #include "carla/sensor/s11n/EpisodeStateSerializer.h"
 #include "carla/sensor/s11n/GnssSerializer.h"
 #include "carla/sensor/s11n/ImageSerializer.h"
+#include "carla/sensor/s11n/OpticalFlowImageSerializer.h"
 #include "carla/sensor/s11n/IMUSerializer.h"
 #include "carla/sensor/s11n/LidarSerializer.h"
 #include "carla/sensor/s11n/NoopSerializer.h"
@@ -34,6 +35,7 @@ class AGnssSensor;
 class AInertialMeasurementUnit;
 class ALaneInvasionSensor;
 class AObstacleDetectionSensor;
+class AOpticalFlowCamera;
 class ARadar;
 class ARayCastSemanticLidar;
 class ARayCastLidar;
@@ -60,6 +62,7 @@ namespace sensor {
     std::pair<AInertialMeasurementUnit *, s11n::IMUSerializer>,
     std::pair<ALaneInvasionSensor *, s11n::NoopSerializer>,
     std::pair<AObstacleDetectionSensor *, s11n::ObstacleDetectionEventSerializer>,
+    std::pair<AOpticalFlowCamera *, s11n::OpticalFlowImageSerializer>,
     std::pair<ARadar *, s11n::RadarSerializer>,
     std::pair<ARayCastSemanticLidar *, s11n::SemanticLidarSerializer>,
     std::pair<ARayCastLidar *, s11n::LidarSerializer>,
@@ -84,6 +87,7 @@ namespace sensor {
 #include "Carla/Sensor/InertialMeasurementUnit.h"
 #include "Carla/Sensor/LaneInvasionSensor.h"
 #include "Carla/Sensor/ObstacleDetectionSensor.h"
+#include "Carla/Sensor/OpticalFlowCamera.h"
 #include "Carla/Sensor/Radar.h"
 #include "Carla/Sensor/RayCastLidar.h"
 #include "Carla/Sensor/RayCastSemanticLidar.h"

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

@@ -46,6 +46,30 @@ namespace data {
 
   static_assert(sizeof(Color) == sizeof(uint32_t), "Invalid color size!");
 
+#pragma pack(push, 1)
+  /// Optical flow pixel format. 2 channel float data.
+  struct OpticalFlowPixel {
+    OpticalFlowPixel() = default;
+    OpticalFlowPixel(const OpticalFlowPixel &) = default;
+
+    OpticalFlowPixel(float x, float y)
+      : x(x), y(y) {}
+
+    OpticalFlowPixel &operator=(const OpticalFlowPixel &) = default;
+
+    bool operator==(const OpticalFlowPixel &rhs) const  {
+      return (x == rhs.x) && (y == rhs.y);
+    }
+
+    bool operator!=(const OpticalFlowPixel &rhs) const  {
+      return !(*this == rhs);
+    }
+
+    float x = 0;
+    float y = 0;
+  };
+#pragma pack(pop)
+
 } // namespace data
 } // namespace sensor
 } // namespace carla

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

@@ -13,9 +13,12 @@ namespace carla {
 namespace sensor {
 namespace data {
 
-  /// An image of 32-bit BGRA colors.
+  /// An image of 32-bit BGRA colors (8-bit channels)
   using Image = ImageTmpl<Color>;
 
+  /// An image of 64-bit BGRA colors (16-bit channels)
+  using OpticalFlowImage = ImageTmpl<OpticalFlowPixel>;
+
 } // namespace data
 } // namespace sensor
 } // namespace carla

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

@@ -9,6 +9,7 @@
 #include "carla/Debug.h"
 #include "carla/sensor/data/Array.h"
 #include "carla/sensor/s11n/ImageSerializer.h"
+#include "carla/sensor/s11n/OpticalFlowImageSerializer.h"
 
 namespace carla {
 namespace sensor {
@@ -21,8 +22,10 @@ namespace data {
   protected:
 
     using Serializer = s11n::ImageSerializer;
+    using SerializerOpticalFlow = s11n::OpticalFlowImageSerializer;
 
     friend Serializer;
+    friend SerializerOpticalFlow;
 
     explicit ImageTmpl(RawData &&data)
       : Super(Serializer::header_offset, std::move(data)) {

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

@@ -0,0 +1,21 @@
+//
+// Created by flo on 09.11.20.
+//
+
+#include "OpticalFlowImageSerializer.h"
+#include "carla/sensor/s11n/OpticalFlowImageSerializer.h"
+
+#include "carla/sensor/data/Image.h"
+
+namespace carla {
+  namespace sensor {
+    namespace s11n {
+
+      SharedPtr<SensorData> OpticalFlowImageSerializer::Deserialize(RawData &&data) {
+        auto image = SharedPtr<data::OpticalFlowImage>(new data::OpticalFlowImage{std::move(data)});
+        return image;
+      }
+
+    } // namespace s11n
+  } // namespace sensor
+} // namespace carla

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

@@ -0,0 +1,59 @@
+//
+// Created by flo on 09.11.20.
+//
+
+#pragma once
+
+#include "carla/Memory.h"
+#include "carla/sensor/RawData.h"
+
+#include <cstdint>
+#include <cstring>
+
+namespace carla {
+  namespace sensor {
+
+    class SensorData;
+
+    namespace s11n {
+
+      /// Serializes image buffers generated by camera sensors.
+      class OpticalFlowImageSerializer {
+      public:
+
+#pragma pack(push, 1)
+        struct ImageHeader {
+          uint32_t width;
+          uint32_t height;
+          float fov_angle;
+        };
+#pragma pack(pop)
+
+        constexpr static auto header_offset = sizeof(ImageHeader);
+
+        static const ImageHeader &DeserializeHeader(const RawData &data) {
+          return *reinterpret_cast<const ImageHeader *>(data.begin());
+        }
+
+        template <typename Sensor>
+        static Buffer Serialize(const Sensor &sensor, Buffer &&bitmap);
+
+        static SharedPtr<SensorData> Deserialize(RawData &&data);
+      };
+
+      template <typename Sensor>
+      inline Buffer OpticalFlowImageSerializer::Serialize(const Sensor &sensor, Buffer &&bitmap) {
+        DEBUG_ASSERT(bitmap.size() > sizeof(ImageHeader));
+        ImageHeader header = {
+            sensor.GetImageWidth(),
+            sensor.GetImageHeight(),
+            sensor.GetFOVAngle()
+        };
+        std::memcpy(bitmap.data(), reinterpret_cast<const void *>(&header), sizeof(header));
+        return std::move(bitmap);
+      }
+
+    } // namespace s11n
+  } // namespace sensor
+} // namespace carla
+

PythonAPI/carla/setup.py

@@ -39,6 +39,8 @@ def walk(folder, file_filter='*'):
             pwd = os.path.dirname(os.path.realpath(__file__))
             pylib = "libboost_python%d%d.a" % (sys.version_info.major,
                                                sys.version_info.minor)
+            numpylib = 'libboost_numpy%d%d.a' % (sys.version_info.major,
+                                               sys.version_info.minor)
             if is_rss_variant_enabled():
                 print('Building AD RSS variant.')
                 extra_link_args = [ os.path.join(pwd, 'dependencies/lib/libcarla_client_rss.a') ]
@@ -84,6 +86,7 @@ def walk(folder, file_filter='*'):
                 extra_link_args += ['-ltbb']
 
             extra_link_args += [os.path.join(pwd, 'dependencies/lib', pylib)]
+            extra_link_args += [os.path.join(pwd, 'dependencies/lib', numpylib)]
 
             if 'TRAVIS' in os.environ and os.environ['TRAVIS'] == 'true':
                 print('Travis CI build detected: disabling PNG support.')
@@ -104,11 +107,14 @@ def walk(folder, file_filter='*'):
         pylib = 'libboost_python%d%d' % (
             sys.version_info.major,
             sys.version_info.minor)
+        numpylib = 'libboost_numpy%d%d' % (
+            sys.version_info.major,
+            sys.version_info.minor)
 
         extra_link_args = ['shlwapi.lib', 'Advapi32.lib', 'ole32.lib', 'shell32.lib']
 
         required_libs = [
-            pylib, 'libboost_filesystem',
+            pylib, numpylib, 'libboost_filesystem',
             'rpc.lib', 'carla_client.lib',
             'libpng.lib', 'zlib.lib',
             'Recast.lib', 'Detour.lib', 'DetourCrowd.lib',

PythonAPI/carla/source/libcarla/SensorData.cpp

@@ -27,6 +27,10 @@
 
 #include <ostream>
 #include <iostream>
+#include <cmath>
+#include <vector>
+#include <algorithm>
+#include <thread>
 
 namespace carla {
 namespace sensor {
@@ -40,6 +44,14 @@ namespace data {
     return out;
   }
 
+  std::ostream &operator<<(std::ostream &out, const OpticalFlowImage &image) {
+    out << "OpticalFlowImage(frame=" << std::to_string(image.GetFrame())
+        << ", timestamp=" << std::to_string(image.GetTimestamp())
+        << ", size=" << std::to_string(image.GetWidth()) << 'x' << std::to_string(image.GetHeight())
+        << ')';
+    return out;
+  }
+
   std::ostream &operator<<(std::ostream &out, const LidarMeasurement &meas) {
     out << "LidarMeasurement(frame=" << std::to_string(meas.GetFrame())
         << ", timestamp=" << std::to_string(meas.GetTimestamp())
@@ -198,6 +210,114 @@ static void ConvertImage(T &self, EColorConverter cc) {
       throw std::invalid_argument("invalid color converter!");
   }
 }
+#ifndef _WIN32
+// method to convert optical flow images to rgb
+static boost::python::numpy::ndarray ColorCodedFlow (
+    carla::sensor::data::OpticalFlowImage& image) {
+  namespace bp = boost::python;
+  namespace bn = boost::python::numpy;
+  namespace csd = carla::sensor::data;
+  constexpr float pi = 3.1415f;
+  constexpr float rad2ang = 360.f/(2.f*pi);
+  std::vector<uint8_t> result;
+  result.resize(image.GetHeight()*image.GetWidth()* 4);
+
+  // lambda for computing batches of pixels
+  auto command = [&] (size_t min_index, size_t max_index) {
+    for (size_t index = min_index; index < max_index; index++) {
+      const csd::OpticalFlowPixel& pixel = image[index];
+      float vx = pixel.x;
+      float vy = pixel.y;
+
+      float angle = 180.f + std::atan2(vy, vx)*rad2ang;
+      if (angle < 0) angle = 360.f + angle;
+      angle = std::fmod(angle, 360.f);
+
+      float norm = std::sqrt(vx*vx + vy*vy);
+      const float shift = 0.999f;
+      const float a = 1.f/std::log(0.1f + shift);
+      float intensity = carla::geom::Math::Clamp(a*std::log(norm + shift), 0.f, 1.f);
+
+      float& H = angle;
+      float  S = 1.f;
+      float V = intensity;
+      float H_60 = H*(1.f/60.f);
+
+      float C = V * S;
+      float X = C*(1.f - std::abs(std::fmod(H_60, 2.f) - 1.f));
+      float m = V - C;
+
+      float r = 0,g = 0,b = 0;
+      unsigned int angle_case = static_cast<unsigned int>(H_60);
+      switch (angle_case) {
+      case 0:
+        r = C;
+        g = X;
+        b = 0;
+        break;
+      case 1:
+        r = X;
+        g = C;
+        b = 0;
+        break;
+      case 2:
+        r = 0;
+        g = C;
+        b = X;
+        break;
+      case 3:
+        r = 0;
+        g = X;
+        b = C;
+        break;
+      case 4:
+        r = X;
+        g = 0;
+        b = C;
+        break;
+      case 5:
+        r = C;
+        g = 0;
+        b = X;
+        break;
+      default:
+        r = 1;
+        g = 1;
+        b = 1;
+        break;
+      }
+
+      uint8_t R = static_cast<uint8_t>((r+m)*255.f);
+      uint8_t G = static_cast<uint8_t>((g+m)*255.f);
+      uint8_t B = static_cast<uint8_t>((b+m)*255.f);
+      result[4*index] = B;
+      result[4*index + 1] = G;
+      result[4*index + 2] = R;
+      result[4*index + 3] = 0;
+    }
+  };
+  size_t num_threads = std::max(8u, std::thread::hardware_concurrency());
+  size_t batch_size = image.size() / num_threads;
+  std::vector<std::thread*> t(num_threads+1);
+
+  for(size_t n = 0; n < num_threads; n++) {
+    t[n] = new std::thread(command, n * batch_size, (n+1) * batch_size);
+  }
+  t[num_threads] = new std::thread(command, num_threads * batch_size, image.size());
+
+  for(size_t n = 0; n <= num_threads; n++) {
+    if(t[n]->joinable()){
+      t[n]->join();
+    }
+    delete t[n];
+  }
+
+  bp::tuple shape = bp::make_tuple(result.size());
+  bp::tuple stride = bp::make_tuple(sizeof(uint8_t));
+  bn::dtype type = bn::dtype::get_builtin<uint8_t>();
+  return bn::from_data(&result[0], type, shape, stride, bp::object()).copy();
+}
+#endif
 
 template <typename T>
 static std::string SaveImageToDisk(T &self, std::string path, EColorConverter cc) {
@@ -272,6 +392,25 @@ void export_sensor_data() {
     .def(self_ns::str(self_ns::self))
   ;
 
+  class_<csd::OpticalFlowImage, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::OpticalFlowImage>>("OpticalFlowImage", no_init)
+    .add_property("width", &csd::OpticalFlowImage::GetWidth)
+    .add_property("height", &csd::OpticalFlowImage::GetHeight)
+    .add_property("fov", &csd::OpticalFlowImage::GetFOVAngle)
+    .add_property("raw_data", &GetRawDataAsBuffer<csd::OpticalFlowImage>)
+    #ifndef _WIN32
+    .def("get_color_coded_flow", &ColorCodedFlow)
+    #endif
+    .def("__len__", &csd::OpticalFlowImage::size)
+    .def("__iter__", iterator<csd::OpticalFlowImage>())
+    .def("__getitem__", +[](const csd::OpticalFlowImage &self, size_t pos) -> csd::OpticalFlowPixel {
+      return self.at(pos);
+    })
+    .def("__setitem__", +[](csd::OpticalFlowImage &self, size_t pos, csd::OpticalFlowPixel color) {
+      self.at(pos) = color;
+    })
+    .def(self_ns::str(self_ns::self))
+  ;
+
   class_<csd::LidarMeasurement, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::LidarMeasurement>>("LidarMeasurement", no_init)
     .add_property("horizontal_angle", &csd::LidarMeasurement::GetHorizontalAngle)
     .add_property("channels", &csd::LidarMeasurement::GetChannelCount)