Added the hdr_publisher that publishes the files needed

event_camera_simulator/esim_ros/src/esim_node.cpp

@@ -1,55 +1,76 @@
+#include "esim/visualization/hdr_publisher.hpp"
+
+#include <esim/data_provider/data_provider_factory.hpp>
 #include <esim/esim/simulator.hpp>
+#include <esim/visualization/adaptive_sampling_benchmark_publisher.hpp>
 #include <esim/visualization/ros_publisher.hpp>
 #include <esim/visualization/rosbag_writer.hpp>
-#include <esim/visualization/adaptive_sampling_benchmark_publisher.hpp>
 #include <esim/visualization/synthetic_optic_flow_publisher.hpp>
-#include <esim/data_provider/data_provider_factory.hpp>
-
-#include <glog/logging.h>
 #include <gflags/gflags.h>
+#include <glog/logging.h>
 
-DEFINE_double(contrast_threshold_pos, 1.0,
-              "Contrast threshold (positive)");
+DEFINE_double(contrast_threshold_pos, 1.0, "Contrast threshold (positive)");
 
-DEFINE_double(contrast_threshold_neg, 1.0,
-              "Contrast threshold  (negative))");
+DEFINE_double(contrast_threshold_neg, 1.0, "Contrast threshold  (negative))");
 
-DEFINE_double(contrast_threshold_sigma_pos, 0.021,
-              "Standard deviation of contrast threshold (positive)");
+DEFINE_double(
+    contrast_threshold_sigma_pos,
+    0.021,
+    "Standard deviation of contrast threshold (positive)"
+);
 
-DEFINE_double(contrast_threshold_sigma_neg, 0.021,
-              "Standard deviation of contrast threshold  (negative))");
+DEFINE_double(
+    contrast_threshold_sigma_neg,
+    0.021,
+    "Standard deviation of contrast threshold  (negative))"
+);
 
-DEFINE_int64(refractory_period_ns, 0,
-             "Refractory period (time during which a pixel cannot fire events just after it fired one), in nanoseconds");
+DEFINE_int64(
+    refractory_period_ns,
+    0,
+    "Refractory period (time during which a pixel cannot fire events just "
+    "after it fired one), in nanoseconds"
+);
 
-DEFINE_double(exposure_time_ms, 10.0,
-              "Exposure time in milliseconds, used to simulate motion blur");
+DEFINE_double(
+    exposure_time_ms,
+    10.0,
+    "Exposure time in milliseconds, used to simulate motion blur"
+);
 
-DEFINE_bool(use_log_image, true,
-            "Whether to convert images to log images in the preprocessing step.");
+DEFINE_bool(
+    use_log_image,
+    true,
+    "Whether to convert images to log images in the preprocessing step."
+);
 
-DEFINE_double(log_eps, 0.001,
-              "Epsilon value used to convert images to log: L = log(eps + I / 255.0).");
+DEFINE_double(
+    log_eps,
+    0.001,
+    "Epsilon value used to convert images to log: L = log(eps + I / 255.0)."
+);
 
-DEFINE_int32(random_seed, 0,
-              "Random seed used to generate the trajectories. If set to 0 the current time(0) is taken as seed.");
+DEFINE_int32(
+    random_seed,
+    0,
+    "Random seed used to generate the trajectories. If set to 0 the current "
+    "time(0) is taken as seed."
+);
 
 using namespace event_camera_simulator;
 
-int main(int argc, char** argv)
-{
+int main(int argc, char** argv) {
     google::InitGoogleLogging(argv[0]);
     google::ParseCommandLineFlags(&argc, &argv, true);
     google::InstallFailureSignalHandler();
     FLAGS_alsologtostderr = true;
     FLAGS_colorlogtostderr = true;
 
-  if (FLAGS_random_seed == 0) FLAGS_random_seed = (unsigned int) time(0);
+    if (FLAGS_random_seed == 0)
+        FLAGS_random_seed = (unsigned int) time(0);
     srand(FLAGS_random_seed);
 
-  DataProviderBase::Ptr data_provider_ =
-      loadDataProviderFromGflags();
+    DataProviderBase::Ptr data_provider_ = loadDataProviderFromGflags();
     CHECK(data_provider_);
 
     EventSimulator::Config event_sim_config;
@@ -61,28 +82,41 @@ int main(int argc, char** argv)
     event_sim_config.use_log_image = FLAGS_use_log_image;
     event_sim_config.log_eps = FLAGS_log_eps;
     std::shared_ptr<Simulator> sim;
-  sim.reset(new Simulator(data_provider_->numCameras(),
+    sim.reset(new Simulator(
+        data_provider_->numCameras(),
         event_sim_config,
-                          FLAGS_exposure_time_ms));
+        FLAGS_exposure_time_ms
+    ));
     CHECK(sim);
 
-  Publisher::Ptr ros_publisher = std::make_shared<RosPublisher>(data_provider_->numCameras());
-  Publisher::Ptr rosbag_writer = RosbagWriter::createBagWriterFromGflags(data_provider_->numCameras());
-  Publisher::Ptr adaptive_sampling_benchmark_publisher
-      = AdaptiveSamplingBenchmarkPublisher::createFromGflags();
+    Publisher::Ptr ros_publisher =
+        std::make_shared<RosPublisher>(data_provider_->numCameras());
+    Publisher::Ptr rosbag_writer =
+        RosbagWriter::createBagWriterFromGflags(data_provider_->numCameras());
+    Publisher::Ptr adaptive_sampling_benchmark_publisher =
+        AdaptiveSamplingBenchmarkPublisher::createFromGflags();
 
-  Publisher::Ptr synthetic_optic_flow_publisher
-      = SyntheticOpticFlowPublisher::createFromGflags();
+    Publisher::Ptr synthetic_optic_flow_publisher =
+        SyntheticOpticFlowPublisher::createFromGflags();
 
-  if(ros_publisher) sim->addPublisher(ros_publisher);
-  if(rosbag_writer) sim->addPublisher(rosbag_writer);
-  if(adaptive_sampling_benchmark_publisher) sim->addPublisher(adaptive_sampling_benchmark_publisher);
-  if(synthetic_optic_flow_publisher) sim->addPublisher(synthetic_optic_flow_publisher);
+    Publisher::Ptr hdr_publisher = HdrPublisher::createFromGflags();
 
-  data_provider_->registerCallback(
-        std::bind(&Simulator::dataProviderCallback, sim.get(),
-                  std::placeholders::_1));
+    if (ros_publisher)
+        sim->addPublisher(ros_publisher);
+    if (rosbag_writer)
+        sim->addPublisher(rosbag_writer);
+    if (adaptive_sampling_benchmark_publisher)
+        sim->addPublisher(adaptive_sampling_benchmark_publisher);
+    if (synthetic_optic_flow_publisher)
+        sim->addPublisher(synthetic_optic_flow_publisher);
+    if (hdr_publisher)
+        sim->addPublisher(hdr_publisher);
+
+    data_provider_->registerCallback(std::bind(
+        &Simulator::dataProviderCallback,
+        sim.get(),
+        std::placeholders::_1
+    ));
 
     data_provider_->spin();
-
 }

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