Dev/eventcamera

.gitignore

@@ -3,6 +3,9 @@ dist
 *.egg-info
 *.so
 __pycache__
+#build
+
+#flightros/params/local.yaml
 
 
 # Gitignore

flightlib/include/flightlib/bridges/unity_bridge.hpp

@@ -6,6 +6,8 @@
 
 // std libs
 #include <unistd.h>
+
+#include <chrono>
 #include <experimental/filesystem>
 #include <fstream>
 #include <map>
@@ -27,8 +29,10 @@
 #include "flightlib/objects/quadrotor.hpp"
 #include "flightlib/objects/static_object.hpp"
 #include "flightlib/objects/unity_camera.hpp"
+#include "flightlib/sensors/event_camera.hpp"
 #include "flightlib/sensors/rgb_camera.hpp"
 
+
 using json = nlohmann::json;
 
 namespace flightlib {
@@ -45,7 +49,9 @@ class UnityBridge {
 
   // public get functions
   bool getRender(const FrameID frame_id);
+  bool trigggerEvents(const FrameID frame_id);
   bool handleOutput();
+  bool handleOutput(bool always);
   bool getPointCloud(PointCloudMessage_t &pointcloud_msg,
                      Scalar time_out = 600.0);
 
@@ -70,13 +76,14 @@ class UnityBridge {
  private:
   bool initializeConnections(void);
 
-  //
   SettingsMessage_t settings_;
   PubMessage_t pub_msg_;
   Logger logger_{"UnityBridge"};
 
   std::vector<std::shared_ptr<Quadrotor>> unity_quadrotors_;
   std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
+  std::vector<std::shared_ptr<EventCamera>> event_cameras_;
+
   std::vector<std::shared_ptr<StaticObject>> static_objects_;
 
   // ZMQ variables and functions

flightlib/include/flightlib/bridges/unity_message_types.hpp

@@ -27,9 +27,14 @@ enum UnityScene {
   TUNELS = 4,
   NATUREFOREST = 3,
   // total number of environment
+  JAPAN = 4,
   SceneNum = 5
 };
 
+
+struct EventsMessage_t {
+  std::vector<Event_t> events;
+};
 // Unity Camera, should not be used alone.
 // has to be attached on a vehicle.
 struct Camera_t {
@@ -54,6 +59,30 @@ struct Camera_t {
                            0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
 };
 
+struct EventCamera_t {
+  std::string ID;
+  // frame Metadata
+  int channels{3};
+  int width{346};
+  int height{260};
+  Scalar fov{70.0f};
+  Scalar depth_scale{0.20};  // 0.xx corresponds to xx cm resolution
+  // metadata
+  bool is_depth{false};
+  int output_index{0};
+  // eventcamera settings
+  float Cm{0.1};
+  float Cp{0.1};
+  float sigma_Cp{0};
+  float sigma_Cm{0};
+  uint64_t refractory_period_ns{0};
+  float log_eps{0.0001};
+  // Transformation matrix from camera to vehicle body 4 x 4
+  // use 1-D vector for json convention
+  std::vector<Scalar> T_BC{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
+                           0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+};
+
 struct Lidar_t {
   std::string ID;
   int num_beams{10};
@@ -76,6 +105,7 @@ struct Vehicle_t {
   std::vector<Scalar> size{1.0, 1.0, 1.0};  // scale
   // sensors attached on the vehicle
   std::vector<Camera_t> cameras;
+  std::vector<EventCamera_t> eventcameras;
   std::vector<Lidar_t> lidars;
   // collision check
   bool has_collision_check = true;
@@ -95,8 +125,6 @@ struct Object_t {
 struct SettingsMessage_t {
   // scene/render settings
   size_t scene_id = UnityScene::WAREHOUSE;
-
-  //
   std::vector<Vehicle_t> vehicles;
   std::vector<Object_t> objects;
 };
@@ -114,9 +142,7 @@ struct Sub_Vehicle_t {
 };
 
 struct SubMessage_t {
-  //
   FrameID frame_id{0};
-  //
   std::vector<Sub_Vehicle_t> sub_vehicles;
 };
 
@@ -147,6 +173,24 @@ inline void to_json(json &j, const Camera_t &o) {
            {"depthScale", o.depth_scale},
            {"outputIndex", o.output_index}};
 }
+// EventCamera_t
+inline void to_json(json &j, const EventCamera_t &o) {
+  j = json{{"ID", o.ID},
+           {"channels", o.channels},
+           {"width", o.width},
+           {"height", o.height},
+           {"fov", o.fov},
+           {"T_BC", o.T_BC},
+           {"isDepth", o.is_depth},
+           {"depthScale", o.depth_scale},
+           {"outputIndex", o.output_index},
+           {"Cm", o.Cm},
+           {"Cp", o.Cp},
+           {"sigma_Cp", o.sigma_Cp},
+           {"sigma_Cm", o.sigma_Cm},
+           {"refractory_period_ns", o.refractory_period_ns},
+           {"log_eps", o.log_eps}};
+}
 
 // Lidar
 inline void to_json(json &j, const Lidar_t &o) {
@@ -164,6 +208,7 @@ inline void to_json(json &j, const Vehicle_t &o) {
            {"rotation", o.rotation},
            {"size", o.size},
            {"cameras", o.cameras},
+           {"eventcameras", o.eventcameras},
            {"lidars", o.lidars},
            {"hasCollisionCheck", o.has_collision_check}};
 }
@@ -203,6 +248,23 @@ inline void from_json(const json &j, SubMessage_t &o) {
   o.sub_vehicles = j.at("pub_vehicles").get<std::vector<Sub_Vehicle_t>>();
 }
 
+inline void from_json(const json &j, Event_t &o) {
+  o.coord_x = j.at("coord_x").get<int>();
+  o.coord_y = j.at("coord_y").get<int>();
+  o.polarity = j.at("polarity").get<int>();
+  o.time = j.at("time").get<float>();
+}
+
+inline void from_json(const json &j, EventsMessage_t &o) {
+  o.events = j.at("events").get<std::vector<Event_t>>();
+}
+
+inline void from_json(const json &j, TimeMessage_t &o) {
+  o.current_time = j.at("current_time").get<int64_t>();
+  o.next_timestep = j.at("next_timestep").get<int64_t>();
+  o.rgb_frame = j.at("rgb_frame").get<bool>();
+}
+
 inline void to_json(json &j, const PointCloudMessage_t &o) {
   j = json{{"range", o.range},
            {"origin", o.origin},
@@ -211,10 +273,22 @@ inline void to_json(json &j, const PointCloudMessage_t &o) {
            {"file_name", o.file_name}};
 }
 
+inline void to_json(json &j, const Event_t &o) {
+  j = json{{"coord_x", o.coord_x},
+           {"coord_y", o.coord_y},
+           {"polarity", o.polarity},
+           {"time", o.time}};
+}
+
+inline void to_json(json &j, const EventsMessage_t &o) {
+  j = json{{"events", o.events}};
+}
+
 // Struct for outputting parsed received messages to handler functions
 struct RenderMessage_t {
   SubMessage_t sub_msg;
   std::vector<cv::Mat> images;
+  // std::vector<Event_t> events;
 };
 
 }  // namespace flightlib

flightlib/include/flightlib/common/types.hpp

@@ -1,6 +1,15 @@
 #pragma once
 
 #include <eigen3/Eigen/Eigen>
+#include <memory>
+#include <opencv2/core/core.hpp>
+
+// #include <cstdio>
+// #include <stdint.h>
+// #include <cinttypes>
+// #include <ze/common/transformation.hpp>
+#define ImageFloatType float
+
 
 namespace flightlib {
 
@@ -9,6 +18,7 @@ namespace flightlib {
 // Define the scalar type used.
 using Scalar = float;  // numpy float32
 
+
 // Define frame id for unity
 using FrameID = uint64_t;
 
@@ -54,6 +64,13 @@ using Quaternion = Eigen::Quaternion<Scalar>;
 template<class Derived>
 using Ref = Eigen::Ref<Derived>;
 
+// Using time for events
+// using Time = ze::int64_t;
+// using uint16_t = ze::uint16_t;
+// using uint16_t =std::unit16_t;
+// using Time = std::int64_t;
+
+
 // // Using `ConstRef` for constant references.
 // template<class Derived>
 // using ConstRef = const Eigen::Ref<const Derived>;
@@ -65,4 +82,44 @@ using Map = Eigen::Map<Derived>;
 static constexpr Scalar Gz = -9.81;
 const Vector<3> GVEC{0.0, 0.0, Gz};
 
+// struct Event
+// {
+//   Event(uint16_t x, uint16_t y, Time t, bool pol)
+//     : x(x),
+//       y(y),
+//       t(t),
+//       pol(pol)
+//   {
+
+//   }
+
+//   uint16_t x;
+//   uint16_t y;
+//   Time t;
+//   bool pol;
+// };
+
+struct Event_t {
+  int coord_x;
+  int coord_y;
+  int polarity;
+  int32_t time;
+};
+
+struct TimeMessage_t {
+  int64_t current_time;
+  int64_t next_timestep;
+  bool rgb_frame;
+};
+using EventsVector = std::vector<Event_t>;
+
+using Image = cv::Mat_<ImageFloatType>;
+using ImagePtr = std::shared_ptr<Image>;
+using ImagePtrVector = std::vector<ImagePtr>;
+
+using RGBImage = cv::Mat;
+using RGBImagePtr = std::shared_ptr<RGBImage>;
+using RGBImagePtrVector = std::vector<RGBImagePtr>;
+
+
 }  // namespace flightlib

flightlib/include/flightlib/objects/quadrotor.hpp

@@ -8,6 +8,7 @@
 #include "flightlib/common/types.hpp"
 #include "flightlib/dynamics/quadrotor_dynamics.hpp"
 #include "flightlib/objects/object_base.hpp"
+#include "flightlib/sensors/event_camera.hpp"
 #include "flightlib/sensors/imu.hpp"
 #include "flightlib/sensors/rgb_camera.hpp"
 
@@ -40,14 +41,19 @@ class Quadrotor : ObjectBase {
   Vector<3> getPosition(void) const;
   Quaternion getQuaternion(void) const;
   std::vector<std::shared_ptr<RGBCamera>> getCameras(void) const;
+  std::vector<std::shared_ptr<EventCamera>> getEventCameras(void) const;
+
   bool getCamera(const size_t cam_id, std::shared_ptr<RGBCamera> camera) const;
+  bool getEventCamera(const size_t cam_id,
+                      std::shared_ptr<EventCamera> camera) const;
   bool getCollision() const;
 
   // public set functions
   bool setState(const QuadState& state);
   bool setCommand(const Command& cmd);
   bool updateDynamics(const QuadrotorDynamics& dynamics);
   bool addRGBCamera(std::shared_ptr<RGBCamera> camera);
+  bool addEventCamera(std::shared_ptr<EventCamera> camera);
 
   // low-level controller
   Vector<4> runFlightCtl(const Scalar sim_dt, const Vector<3>& omega,
@@ -71,6 +77,7 @@ class Quadrotor : ObjectBase {
   IMU imu_;
   std::unique_ptr<IntegratorRK4> integrator_ptr_;
   std::vector<std::shared_ptr<RGBCamera>> rgb_cameras_;
+  std::vector<std::shared_ptr<EventCamera>> event_cameras_;
 
   // quad control command
   Command cmd_;