diff --git a/event_camera_simulator/esim_ros/src/esim_node.cpp b/event_camera_simulator/esim_ros/src/esim_node.cpp index c042d78..e0b373f 100644 --- a/event_camera_simulator/esim_ros/src/esim_node.cpp +++ b/event_camera_simulator/esim_ros/src/esim_node.cpp @@ -1,88 +1,122 @@ +#include "esim/visualization/hdr_publisher.hpp" + +#include #include +#include #include #include -#include #include -#include - -#include #include +#include -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) -{ - google::InitGoogleLogging(argv[0]); - google::ParseCommandLineFlags(&argc, &argv, true); - google::InstallFailureSignalHandler(); - FLAGS_alsologtostderr = true; - FLAGS_colorlogtostderr = true; +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); - srand(FLAGS_random_seed); + if (FLAGS_random_seed == 0) + FLAGS_random_seed = (unsigned int) time(0); + srand(FLAGS_random_seed); - DataProviderBase::Ptr data_provider_ = - loadDataProviderFromGflags(); - CHECK(data_provider_); + DataProviderBase::Ptr data_provider_ = loadDataProviderFromGflags(); + CHECK(data_provider_); - EventSimulator::Config event_sim_config; - event_sim_config.Cp = FLAGS_contrast_threshold_pos; - event_sim_config.Cm = FLAGS_contrast_threshold_neg; - event_sim_config.sigma_Cp = FLAGS_contrast_threshold_sigma_pos; - event_sim_config.sigma_Cm = FLAGS_contrast_threshold_sigma_neg; - event_sim_config.refractory_period_ns = FLAGS_refractory_period_ns; - event_sim_config.use_log_image = FLAGS_use_log_image; - event_sim_config.log_eps = FLAGS_log_eps; - std::shared_ptr sim; - sim.reset(new Simulator(data_provider_->numCameras(), - event_sim_config, - FLAGS_exposure_time_ms)); - CHECK(sim); + EventSimulator::Config event_sim_config; + event_sim_config.Cp = FLAGS_contrast_threshold_pos; + event_sim_config.Cm = FLAGS_contrast_threshold_neg; + event_sim_config.sigma_Cp = FLAGS_contrast_threshold_sigma_pos; + event_sim_config.sigma_Cm = FLAGS_contrast_threshold_sigma_neg; + event_sim_config.refractory_period_ns = FLAGS_refractory_period_ns; + event_sim_config.use_log_image = FLAGS_use_log_image; + event_sim_config.log_eps = FLAGS_log_eps; + std::shared_ptr sim; + sim.reset(new Simulator( + data_provider_->numCameras(), + event_sim_config, + FLAGS_exposure_time_ms + )); + CHECK(sim); - Publisher::Ptr ros_publisher = std::make_shared(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(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_->spin(); + data_provider_->registerCallback(std::bind( + &Simulator::dataProviderCallback, + sim.get(), + std::placeholders::_1 + )); + data_provider_->spin(); } diff --git a/event_camera_simulator/esim_visualization/CMakeLists.txt b/event_camera_simulator/esim_visualization/CMakeLists.txt index 52c17cc..537fe93 100644 --- a/event_camera_simulator/esim_visualization/CMakeLists.txt +++ b/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}) diff --git a/event_camera_simulator/esim_visualization/include/esim/visualization/hdr_publisher.hpp b/event_camera_simulator/esim_visualization/include/esim/visualization/hdr_publisher.hpp new file mode 100644 index 0000000..fc7334f --- /dev/null +++ b/event_camera_simulator/esim_visualization/include/esim/visualization/hdr_publisher.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include +#include +#include + +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 exposures; // amount of exposure for each frame + // Events events_; // buffer containing all the events since the + // beginning + }; + +} // namespace event_camera_simulator diff --git a/event_camera_simulator/esim_visualization/src/hdr_publisher.cpp b/event_camera_simulator/esim_visualization/src/hdr_publisher.cpp new file mode 100644 index 0000000..03f1538 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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(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