Added the hdr_publisher that publishes the files needed

This commit is contained in:
Arnaud Fauconnet 2023-07-17 11:19:12 +02:00
parent 8280d179cd
commit 2cb8b1db41
4 changed files with 296 additions and 61 deletions

View File

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

View File

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

View File

@ -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

View File

@ -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