Added the hdr_publisher that publishes the files needed

event_camera_simulator/esim_visualization/CMakeLists.txt

@@ -22,6 +22,7 @@ set(HEADERS
     include/esim/visualization/rosbag_writer.hpp
     include/esim/visualization/adaptive_sampling_benchmark_publisher.hpp
     include/esim/visualization/synthetic_optic_flow_publisher.hpp
+    include/esim/visualization/hdr_publisher.hpp
 )
 
 set(SOURCES
@@ -30,6 +31,7 @@ set(SOURCES
     src/rosbag_writer.cpp
     src/adaptive_sampling_benchmark_publisher.cpp
     src/synthetic_optic_flow_publisher.cpp
+    src/hdr_publisher.cpp
 )
 
 cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})

event_camera_simulator/esim_visualization/include/esim/visualization/hdr_publisher.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <esim/common/types.hpp>
+#include <esim/visualization/publisher_interface.hpp>
+#include <fstream>
+
+namespace event_camera_simulator {
+
+    class HdrPublisher : public Publisher {
+      public:
+        HdrPublisher(const std::string& output_folder);
+
+        ~HdrPublisher();
+
+        virtual void
+        imageCallback(const ImagePtrVector& images, Time t) override;
+
+        virtual void eventsCallback(const EventsVector& events) override;
+
+        virtual void poseCallback(
+            const Transformation& T_W_B,
+            const TransformationVector& T_W_Cs,
+            Time t
+        ) override;
+
+        virtual void opticFlowCallback(
+            const OpticFlowPtrVector& optic_flows, Time t
+        ) override {}
+
+        virtual void imageCorruptedCallback(
+            const ImagePtrVector& corrupted_images, Time t
+        ) override {}
+
+        virtual void
+        depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
+
+        virtual void twistCallback(
+            const AngularVelocityVector& ws,
+            const LinearVelocityVector& vs,
+            Time t
+        ) override{};
+
+        virtual void
+        imuCallback(const Vector3& acc, const Vector3& gyr, Time t) override{};
+
+        virtual void cameraInfoCallback(
+            const ze::CameraRig::Ptr& camera_rig, Time t
+        ) override{};
+
+        virtual void pointcloudCallback(
+            const PointCloudVector& pointclouds, Time t
+        ) override {}
+
+        static Publisher::Ptr createFromGflags();
+
+      private:
+        std::string output_folder_;
+        std::ofstream exposures_file_;
+        std::ofstream poses_file_;
+        std::ofstream events_file_;
+
+        // ImagePtrVector images;        // images with motion blur
+        // TransformationVector poses;   // poses of the camera
+        // std::vector<float> exposures; // amount of exposure for each frame
+        // Events events_; // buffer containing all the events since the
+        // beginning
+    };
+
+} // namespace event_camera_simulator

event_camera_simulator/esim_visualization/src/hdr_publisher.cpp

@@ -0,0 +1,130 @@
+#include "esim/common/types.hpp"
+#include "kindr/minimal/rotation-quaternion.h"
+
+#include <esim/common/utils.hpp>
+#include <esim/visualization/hdr_publisher.hpp>
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+#include <iomanip>
+#include <opencv2/highgui/highgui.hpp>
+#include <sys/stat.h>
+#include <ze/common/file_utils.hpp>
+#include <ze/common/path_utils.hpp>
+#include <ze/common/time_conversions.hpp>
+
+DEFINE_string(
+    hdr_output_folder,
+    "/home/arno/sim_ws/out",
+    "Folder in which to output the events."
+);
+
+namespace event_camera_simulator {
+
+    /**
+     * This publisher was designed with the purpose of generating simulation
+     * data with ground truth labels, for the task of optic flow estimation.
+     *
+     * It assumes that it will receive a relatively small sequence of events
+     * (corresponding, for example, to all the events in between two frames),
+     * and will write all the events to disk in its destructor, in three forms:
+     *   - an "events.txt" file that contains all the events in "t x y pol"
+     * format (one event per line)
+     *   - an "event_count.png" image that whose first two channels contain the
+     * counts of the positive (resp. negative) event counts at each pixel
+     *   - two "timestamps images" in which each pixel contains the timestamp at
+     * the last event that fell on the pixel. (since the timestamp is a floating
+     * point value, it is split in 3 8-bit values so that the timestamp images
+     *      can be saved in a single 3-channel image).
+     */
+
+    HdrPublisher::HdrPublisher(const std::string& output_folder)
+        : output_folder_(output_folder) {
+        if (!ze::isDir(output_folder_))
+            mkdir(output_folder_.c_str(), 0777);
+        if (!ze::isDir(output_folder_ + "/frames"))
+            mkdir((output_folder_ + "/frames").c_str(), 0777);
+
+        ze::openOutputFileStream(
+            ze::joinPath(output_folder, "exposures.csv"),
+            &exposures_file_
+        );
+
+        exposures_file_ << "timestamp,frame_number,exposure" << std::endl;
+        exposures_file_.close();
+
+        ze::openOutputFileStream(
+            ze::joinPath(output_folder, "poses.csv"),
+            &poses_file_
+        );
+        // Set the headers of the poses csv file
+        poses_file_ << "timestamp,";
+        poses_file_ << "px,";
+        poses_file_ << "py,";
+        poses_file_ << "pz,";
+        poses_file_ << "qx,";
+        poses_file_ << "qy,";
+        poses_file_ << "qz,";
+        poses_file_ << "qw";
+        poses_file_ << std::endl;
+
+        ze::openOutputFileStream(
+            ze::joinPath(output_folder, "events.csv"),
+            &events_file_
+        );
+        // Set the headers of the events csv file
+        events_file_ << "timestamp,";
+        events_file_ << "polarity,";
+        events_file_ << "x,";
+        events_file_ << "y";
+        events_file_ << std::endl;
+    }
+
+    Publisher::Ptr HdrPublisher::createFromGflags() {
+        if (FLAGS_hdr_output_folder == "") {
+            LOG(WARNING) << "Empty output folder string: will not write "
+                            "hdr files";
+            return nullptr;
+        }
+
+        return std::make_shared<HdrPublisher>(FLAGS_hdr_output_folder);
+    }
+
+    HdrPublisher::~HdrPublisher() {
+        events_file_.close();
+        poses_file_.close();
+    }
+
+    void HdrPublisher::poseCallback(
+        const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t
+    ) {
+        Vector3 p = T_W_B.getPosition();
+        poses_file_ << t << "," << p.x() << "," << p.y() << "," << p.z() << ",";
+
+        auto rotation = T_W_B.getRotation();
+        poses_file_ << rotation.x() << "," << rotation.y() << ","
+                    << rotation.z() << "," << rotation.w() << std::endl;
+    }
+
+    void HdrPublisher::imageCallback(const ImagePtrVector& images, Time t) {
+        CHECK_EQ(images.size(), 1);
+        static uint frame_number = 0;
+        std::stringstream ss;
+        ss << output_folder_ << "/frames/frame_" << std::setfill('0')
+           << std::setw(5) << frame_number++ << ".exr";
+        std::string frame_path = ss.str();
+
+        cv::imwrite(frame_path, *images[0]);
+    }
+
+    void HdrPublisher::eventsCallback(const EventsVector& events) {
+        CHECK_EQ(events.size(), 1);
+
+        // Simply aggregate the events into the events_ buffer.
+        // At the destruction of this object, everything will be saved to disk.
+        for (const Event& e : events[0]) {
+            events_file_ << e.t << "," << (e.pol ? 1 : -1) << "," << e.x << ","
+                         << e.y << std::endl;
+        }
+    }
+
+} // namespace event_camera_simulator