mirror of
https://github.com/karma-riuk/hdr_esim.git
synced 2025-06-19 10:57:51 +02:00
Reformated the project to make it more readable (to me)
This commit is contained in:
Arnaud Fauconnet
@ -2,44 +2,68 @@
|
||||
|
||||
#include <esim/common/types.hpp>
|
||||
#include <esim/visualization/publisher_interface.hpp>
|
||||
|
||||
#include <fstream>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class AdaptiveSamplingBenchmarkPublisher : public Publisher
|
||||
{
|
||||
public:
|
||||
class AdaptiveSamplingBenchmarkPublisher : public Publisher {
|
||||
public:
|
||||
using PixelLocation = std::pair<int, int>;
|
||||
using PixelLocations = std::vector<PixelLocation>;
|
||||
|
||||
using PixelLocation = std::pair<int,int>;
|
||||
using PixelLocations = std::vector<PixelLocation>;
|
||||
AdaptiveSamplingBenchmarkPublisher(
|
||||
const std::string& benchmark_folder,
|
||||
const std::string& pixels_to_record_filename
|
||||
);
|
||||
|
||||
AdaptiveSamplingBenchmarkPublisher(const std::string &benchmark_folder,
|
||||
const std::string &pixels_to_record_filename);
|
||||
~AdaptiveSamplingBenchmarkPublisher();
|
||||
|
||||
~AdaptiveSamplingBenchmarkPublisher();
|
||||
virtual void
|
||||
imageCallback(const ImagePtrVector& images, Time t) override;
|
||||
virtual void eventsCallback(const EventsVector& events) override;
|
||||
virtual void opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, Time t
|
||||
) override;
|
||||
|
||||
virtual void imageCallback(const ImagePtrVector& images, Time t) override;
|
||||
virtual void eventsCallback(const EventsVector& events) override;
|
||||
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) override;
|
||||
virtual void imageCorruptedCallback(
|
||||
const ImagePtrVector& corrupted_images, Time t
|
||||
) override {}
|
||||
|
||||
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override {}
|
||||
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
|
||||
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, 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 {}
|
||||
virtual void
|
||||
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
|
||||
|
||||
static Publisher::Ptr createFromGflags();
|
||||
virtual void poseCallback(
|
||||
const Transformation& T_W_B,
|
||||
const TransformationVector& T_W_Cs,
|
||||
Time t
|
||||
) override {}
|
||||
|
||||
private:
|
||||
std::ofstream events_file_;
|
||||
std::ofstream images_file_;
|
||||
std::ofstream pixel_intensities_file_;
|
||||
std::ofstream optic_flows_file_;
|
||||
size_t image_index_;
|
||||
PixelLocations pixels_to_record_;
|
||||
};
|
||||
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::ofstream events_file_;
|
||||
std::ofstream images_file_;
|
||||
std::ofstream pixel_intensities_file_;
|
||||
std::ofstream optic_flows_file_;
|
||||
size_t image_index_;
|
||||
PixelLocations pixels_to_record_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -5,25 +5,47 @@
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class Publisher
|
||||
{
|
||||
public:
|
||||
ZE_POINTER_TYPEDEFS(Publisher);
|
||||
class Publisher {
|
||||
public:
|
||||
ZE_POINTER_TYPEDEFS(Publisher);
|
||||
|
||||
Publisher() = default;
|
||||
virtual ~Publisher() = default;
|
||||
Publisher() = default;
|
||||
virtual ~Publisher() = default;
|
||||
|
||||
virtual void imageCallback(const ImagePtrVector& images, Time t) {}
|
||||
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) {}
|
||||
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {}
|
||||
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) {}
|
||||
virtual void eventsCallback(const EventsVector& events) {}
|
||||
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t) {}
|
||||
virtual void twistCallback(const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t) {}
|
||||
virtual void imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {}
|
||||
virtual void cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) {}
|
||||
virtual void pointcloudCallback(const PointCloudVector& pointclouds, Time t) {}
|
||||
virtual void imageCallback(const ImagePtrVector& images, Time t) {}
|
||||
|
||||
};
|
||||
virtual void
|
||||
imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) {
|
||||
}
|
||||
|
||||
virtual void
|
||||
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {}
|
||||
|
||||
virtual void
|
||||
opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) {}
|
||||
|
||||
virtual void eventsCallback(const EventsVector& events) {}
|
||||
|
||||
virtual void poseCallback(
|
||||
const Transformation& T_W_B,
|
||||
const TransformationVector& T_W_Cs,
|
||||
Time t
|
||||
) {}
|
||||
|
||||
virtual void twistCallback(
|
||||
const AngularVelocityVector& ws,
|
||||
const LinearVelocityVector& vs,
|
||||
Time t
|
||||
) {}
|
||||
|
||||
virtual void
|
||||
imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {}
|
||||
|
||||
virtual void
|
||||
cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t) {}
|
||||
|
||||
virtual void
|
||||
pointcloudCallback(const PointCloudVector& pointclouds, Time t) {}
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -2,58 +2,75 @@
|
||||
|
||||
#include <esim/common/types.hpp>
|
||||
#include <esim/visualization/publisher_interface.hpp>
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <image_transport/image_transport.h>
|
||||
#include <ros/ros.h>
|
||||
#include <tf/tf.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class RosPublisher : public Publisher
|
||||
{
|
||||
public:
|
||||
RosPublisher(size_t num_cameras);
|
||||
~RosPublisher();
|
||||
class RosPublisher : public Publisher {
|
||||
public:
|
||||
RosPublisher(size_t num_cameras);
|
||||
~RosPublisher();
|
||||
|
||||
virtual void imageCallback(const ImagePtrVector& images, Time t) override;
|
||||
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override;
|
||||
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
|
||||
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, 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 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;
|
||||
virtual void
|
||||
imageCallback(const ImagePtrVector& images, Time t) override;
|
||||
virtual void imageCorruptedCallback(
|
||||
const ImagePtrVector& corrupted_images, Time t
|
||||
) override;
|
||||
virtual void
|
||||
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
|
||||
virtual void opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, 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 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;
|
||||
|
||||
private:
|
||||
size_t num_cameras_;
|
||||
std::vector<cv::Size> sensor_sizes_;
|
||||
private:
|
||||
size_t num_cameras_;
|
||||
std::vector<cv::Size> sensor_sizes_;
|
||||
|
||||
std::shared_ptr<ros::NodeHandle> nh_;
|
||||
std::shared_ptr<image_transport::ImageTransport> it_;
|
||||
std::shared_ptr<ros::NodeHandle> nh_;
|
||||
std::shared_ptr<image_transport::ImageTransport> it_;
|
||||
|
||||
std::vector< std::shared_ptr<ros::Publisher> > event_pub_;
|
||||
std::shared_ptr<ros::Publisher> pose_pub_;
|
||||
std::shared_ptr<ros::Publisher> imu_pub_;
|
||||
std::vector< std::shared_ptr<ros::Publisher> > pointcloud_pub_;
|
||||
std::vector< std::shared_ptr<ros::Publisher> > camera_info_pub_;
|
||||
std::vector< std::shared_ptr<image_transport::Publisher> > image_pub_;
|
||||
std::vector< std::shared_ptr<image_transport::Publisher> > image_corrupted_pub_;
|
||||
std::vector< std::shared_ptr<image_transport::Publisher> > depthmap_pub_;
|
||||
std::vector< std::shared_ptr<ros::Publisher> > optic_flow_pub_;
|
||||
std::vector< std::shared_ptr<ros::Publisher> > twist_pub_;
|
||||
std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;
|
||||
std::vector<std::shared_ptr<ros::Publisher>> event_pub_;
|
||||
std::shared_ptr<ros::Publisher> pose_pub_;
|
||||
std::shared_ptr<ros::Publisher> imu_pub_;
|
||||
std::vector<std::shared_ptr<ros::Publisher>> pointcloud_pub_;
|
||||
std::vector<std::shared_ptr<ros::Publisher>> camera_info_pub_;
|
||||
std::vector<std::shared_ptr<image_transport::Publisher>> image_pub_;
|
||||
std::vector<std::shared_ptr<image_transport::Publisher>>
|
||||
image_corrupted_pub_;
|
||||
std::vector<std::shared_ptr<image_transport::Publisher>> depthmap_pub_;
|
||||
std::vector<std::shared_ptr<ros::Publisher>> optic_flow_pub_;
|
||||
std::vector<std::shared_ptr<ros::Publisher>> twist_pub_;
|
||||
std::shared_ptr<tf::TransformBroadcaster> tf_broadcaster_;
|
||||
|
||||
Time last_published_camera_info_time_;
|
||||
Time last_published_image_time_;
|
||||
Time last_published_corrupted_image_time_;
|
||||
Time last_published_depthmap_time_;
|
||||
Time last_published_optic_flow_time_;
|
||||
Time last_published_pointcloud_time_;
|
||||
|
||||
};
|
||||
Time last_published_camera_info_time_;
|
||||
Time last_published_image_time_;
|
||||
Time last_published_corrupted_image_time_;
|
||||
Time last_published_depthmap_time_;
|
||||
Time last_published_optic_flow_time_;
|
||||
Time last_published_pointcloud_time_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -1,54 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include <esim/common/types.hpp>
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <dvs_msgs/EventArray.h>
|
||||
#include <esim/common/types.hpp>
|
||||
#include <esim_msgs/OpticFlow.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TwistStamped.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_ros/point_cloud.h>
|
||||
#include <sensor_msgs/CameraInfo.h>
|
||||
#include <esim_msgs/OpticFlow.h>
|
||||
#include <sensor_msgs/Image.h>
|
||||
#include <sensor_msgs/Imu.h>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
inline std::string getTopicName(int i, const std::string& suffix)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i << "/" << suffix;
|
||||
return ss.str();
|
||||
}
|
||||
inline std::string getTopicName(int i, const std::string& suffix) {
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i << "/" << suffix;
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
inline std::string getTopicName(const std::string& prefix, int i, const std::string& suffix)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << prefix << "/" << getTopicName(i, suffix);
|
||||
return ss.str();
|
||||
}
|
||||
inline std::string
|
||||
getTopicName(const std::string& prefix, int i, const std::string& suffix) {
|
||||
std::stringstream ss;
|
||||
ss << prefix << "/" << getTopicName(i, suffix);
|
||||
return ss.str();
|
||||
}
|
||||
|
||||
inline ros::Time toRosTime(Time t)
|
||||
{
|
||||
ros::Time ros_time;
|
||||
ros_time.fromNSec(t);
|
||||
return ros_time;
|
||||
}
|
||||
inline ros::Time toRosTime(Time t) {
|
||||
ros::Time ros_time;
|
||||
ros_time.fromNSec(t);
|
||||
return ros_time;
|
||||
}
|
||||
|
||||
void pointCloudToMsg(const PointCloud& pointcloud, const std::string& frame_id, Time t,pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg);
|
||||
void pointCloudToMsg(
|
||||
const PointCloud& pointcloud,
|
||||
const std::string& frame_id,
|
||||
Time t,
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg
|
||||
);
|
||||
|
||||
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg);
|
||||
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg);
|
||||
|
||||
void depthmapToMsg(const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg);
|
||||
void
|
||||
depthmapToMsg(const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg);
|
||||
|
||||
void opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg);
|
||||
void
|
||||
opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg);
|
||||
|
||||
void eventsToMsg(const Events& events, int width, int height, dvs_msgs::EventArrayPtr& msg);
|
||||
void eventsToMsg(
|
||||
const Events& events,
|
||||
int width,
|
||||
int height,
|
||||
dvs_msgs::EventArrayPtr& msg
|
||||
);
|
||||
|
||||
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t);
|
||||
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t);
|
||||
|
||||
geometry_msgs::TwistStamped twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t);
|
||||
|
||||
void cameraToMsg(const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg);
|
||||
geometry_msgs::TwistStamped
|
||||
twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t);
|
||||
|
||||
void cameraToMsg(
|
||||
const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg
|
||||
);
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -6,40 +6,56 @@
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class RosbagWriter : public Publisher
|
||||
{
|
||||
public:
|
||||
RosbagWriter(const std::string& path_to_output_bag,
|
||||
size_t num_cameras);
|
||||
~RosbagWriter();
|
||||
class RosbagWriter : public Publisher {
|
||||
public:
|
||||
RosbagWriter(const std::string& path_to_output_bag, size_t num_cameras);
|
||||
~RosbagWriter();
|
||||
|
||||
virtual void imageCallback(const ImagePtrVector& images, Time t) override;
|
||||
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override;
|
||||
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
|
||||
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, 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 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;
|
||||
virtual void
|
||||
imageCallback(const ImagePtrVector& images, Time t) override;
|
||||
virtual void imageCorruptedCallback(
|
||||
const ImagePtrVector& corrupted_images, Time t
|
||||
) override;
|
||||
virtual void
|
||||
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override;
|
||||
virtual void opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, 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 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 createBagWriterFromGflags(size_t num_cameras);
|
||||
static Publisher::Ptr createBagWriterFromGflags(size_t num_cameras);
|
||||
|
||||
private:
|
||||
size_t num_cameras_;
|
||||
std::vector<cv::Size> sensor_sizes_;
|
||||
rosbag::Bag bag_;
|
||||
private:
|
||||
size_t num_cameras_;
|
||||
std::vector<cv::Size> sensor_sizes_;
|
||||
rosbag::Bag bag_;
|
||||
|
||||
const std::string topic_name_prefix_ = "";
|
||||
const std::string topic_name_prefix_ = "";
|
||||
|
||||
Time last_published_camera_info_time_;
|
||||
Time last_published_image_time_;
|
||||
Time last_published_corrupted_image_time_;
|
||||
Time last_published_depthmap_time_;
|
||||
Time last_published_optic_flow_time_;
|
||||
Time last_published_pointcloud_time_;
|
||||
|
||||
};
|
||||
Time last_published_camera_info_time_;
|
||||
Time last_published_image_time_;
|
||||
Time last_published_corrupted_image_time_;
|
||||
Time last_published_depthmap_time_;
|
||||
Time last_published_optic_flow_time_;
|
||||
Time last_published_pointcloud_time_;
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -2,44 +2,66 @@
|
||||
|
||||
#include <esim/common/types.hpp>
|
||||
#include <esim/visualization/publisher_interface.hpp>
|
||||
|
||||
#include <fstream>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
class SyntheticOpticFlowPublisher : public Publisher
|
||||
{
|
||||
public:
|
||||
SyntheticOpticFlowPublisher(const std::string &output_folder);
|
||||
class SyntheticOpticFlowPublisher : public Publisher {
|
||||
public:
|
||||
SyntheticOpticFlowPublisher(const std::string& output_folder);
|
||||
|
||||
~SyntheticOpticFlowPublisher();
|
||||
~SyntheticOpticFlowPublisher();
|
||||
|
||||
virtual void imageCallback(const ImagePtrVector& images, Time t) override {
|
||||
CHECK_EQ(images.size(), 1);
|
||||
if(sensor_size_.width == 0 || sensor_size_.height == 0)
|
||||
{
|
||||
sensor_size_ = images[0]->size();
|
||||
}
|
||||
}
|
||||
virtual void
|
||||
imageCallback(const ImagePtrVector& images, Time t) override {
|
||||
CHECK_EQ(images.size(), 1);
|
||||
if (sensor_size_.width == 0 || sensor_size_.height == 0)
|
||||
sensor_size_ = images[0]->size();
|
||||
}
|
||||
|
||||
virtual void eventsCallback(const EventsVector& events) override;
|
||||
virtual void opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t) override {}
|
||||
virtual void eventsCallback(const EventsVector& events) override;
|
||||
|
||||
virtual void imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t) override {}
|
||||
virtual void depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
|
||||
virtual void poseCallback(const Transformation& T_W_B, const TransformationVector& T_W_Cs, 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 {}
|
||||
virtual void opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, Time t
|
||||
) override {}
|
||||
|
||||
static Publisher::Ptr createFromGflags();
|
||||
virtual void imageCorruptedCallback(
|
||||
const ImagePtrVector& corrupted_images, Time t
|
||||
) override {}
|
||||
|
||||
private:
|
||||
std::string output_folder_;
|
||||
cv::Size sensor_size_;
|
||||
std::ofstream events_file_;
|
||||
Events events_; // buffer containing all the events since the beginning
|
||||
};
|
||||
virtual void
|
||||
depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) override {}
|
||||
|
||||
virtual void poseCallback(
|
||||
const Transformation& T_W_B,
|
||||
const TransformationVector& T_W_Cs,
|
||||
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_;
|
||||
cv::Size sensor_size_;
|
||||
std::ofstream events_file_;
|
||||
Events events_; // buffer containing all the events since the beginning
|
||||
};
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -1,135 +1,151 @@
|
||||
#include <esim/visualization/adaptive_sampling_benchmark_publisher.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <ze/common/path_utils.hpp>
|
||||
#include <ze/common/file_utils.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <esim/visualization/adaptive_sampling_benchmark_publisher.hpp>
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <ze/common/file_utils.hpp>
|
||||
#include <ze/common/path_utils.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
|
||||
DEFINE_string(adaptive_sampling_benchmark_folder, "",
|
||||
"Folder in which to output the results.");
|
||||
DEFINE_string(
|
||||
adaptive_sampling_benchmark_folder,
|
||||
"",
|
||||
"Folder in which to output the results."
|
||||
);
|
||||
|
||||
DEFINE_string(adaptive_sampling_benchmark_pixels_to_record_file, "",
|
||||
"File containing the pixel locations to record.");
|
||||
DEFINE_string(
|
||||
adaptive_sampling_benchmark_pixels_to_record_file,
|
||||
"",
|
||||
"File containing the pixel locations to record."
|
||||
);
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
AdaptiveSamplingBenchmarkPublisher::AdaptiveSamplingBenchmarkPublisher(const std::string& benchmark_folder,
|
||||
const std::string& pixels_to_record_filename)
|
||||
: image_index_(0)
|
||||
{
|
||||
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "events.txt"),
|
||||
&events_file_);
|
||||
AdaptiveSamplingBenchmarkPublisher::AdaptiveSamplingBenchmarkPublisher(
|
||||
const std::string& benchmark_folder,
|
||||
const std::string& pixels_to_record_filename
|
||||
)
|
||||
: image_index_(0) {
|
||||
ze::openOutputFileStream(
|
||||
ze::joinPath(benchmark_folder, "events.txt"),
|
||||
&events_file_
|
||||
);
|
||||
|
||||
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "images.txt"),
|
||||
&images_file_);
|
||||
ze::openOutputFileStream(
|
||||
ze::joinPath(benchmark_folder, "images.txt"),
|
||||
&images_file_
|
||||
);
|
||||
|
||||
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "pixel_intensities.txt"),
|
||||
&pixel_intensities_file_);
|
||||
ze::openOutputFileStream(
|
||||
ze::joinPath(benchmark_folder, "pixel_intensities.txt"),
|
||||
&pixel_intensities_file_
|
||||
);
|
||||
|
||||
ze::openOutputFileStream(ze::joinPath(benchmark_folder, "optic_flows.txt"),
|
||||
&optic_flows_file_);
|
||||
ze::openOutputFileStream(
|
||||
ze::joinPath(benchmark_folder, "optic_flows.txt"),
|
||||
&optic_flows_file_
|
||||
);
|
||||
|
||||
// Load and parse the file containing the list of pixel locations
|
||||
// whose intensity values to record
|
||||
std::ifstream pixels_to_record_file;
|
||||
if(pixels_to_record_filename != "")
|
||||
{
|
||||
ze::openFileStream(pixels_to_record_filename, &pixels_to_record_file);
|
||||
// Load and parse the file containing the list of pixel locations
|
||||
// whose intensity values to record
|
||||
std::ifstream pixels_to_record_file;
|
||||
if (pixels_to_record_filename != "") {
|
||||
ze::openFileStream(
|
||||
pixels_to_record_filename,
|
||||
&pixels_to_record_file
|
||||
);
|
||||
|
||||
int x, y;
|
||||
LOG(INFO) << "Pixels that will be recorded: ";
|
||||
while(pixels_to_record_file >> x >> y)
|
||||
{
|
||||
LOG(INFO) << x << " , " << y;
|
||||
pixels_to_record_.push_back(PixelLocation(x,y));
|
||||
int x, y;
|
||||
LOG(INFO) << "Pixels that will be recorded: ";
|
||||
while (pixels_to_record_file >> x >> y) {
|
||||
LOG(INFO) << x << " , " << y;
|
||||
pixels_to_record_.push_back(PixelLocation(x, y));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Publisher::Ptr AdaptiveSamplingBenchmarkPublisher::createFromGflags()
|
||||
{
|
||||
if(FLAGS_adaptive_sampling_benchmark_folder == "")
|
||||
{
|
||||
LOG(WARNING) << "Empty benchmark folder string: will not write benchmark files";
|
||||
return nullptr;
|
||||
}
|
||||
Publisher::Ptr AdaptiveSamplingBenchmarkPublisher::createFromGflags() {
|
||||
if (FLAGS_adaptive_sampling_benchmark_folder == "") {
|
||||
LOG(WARNING) << "Empty benchmark folder string: will not write "
|
||||
"benchmark files";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return std::make_shared<AdaptiveSamplingBenchmarkPublisher>(FLAGS_adaptive_sampling_benchmark_folder,
|
||||
FLAGS_adaptive_sampling_benchmark_pixels_to_record_file);
|
||||
}
|
||||
return std::make_shared<AdaptiveSamplingBenchmarkPublisher>(
|
||||
FLAGS_adaptive_sampling_benchmark_folder,
|
||||
FLAGS_adaptive_sampling_benchmark_pixels_to_record_file
|
||||
);
|
||||
}
|
||||
|
||||
AdaptiveSamplingBenchmarkPublisher::~AdaptiveSamplingBenchmarkPublisher()
|
||||
{
|
||||
// finish writing files
|
||||
events_file_.close();
|
||||
images_file_.close();
|
||||
pixel_intensities_file_.close();
|
||||
optic_flows_file_.close();
|
||||
}
|
||||
AdaptiveSamplingBenchmarkPublisher::~AdaptiveSamplingBenchmarkPublisher() {
|
||||
// finish writing files
|
||||
events_file_.close();
|
||||
images_file_.close();
|
||||
pixel_intensities_file_.close();
|
||||
optic_flows_file_.close();
|
||||
}
|
||||
|
||||
void AdaptiveSamplingBenchmarkPublisher::imageCallback(const ImagePtrVector& images, Time t)
|
||||
{
|
||||
CHECK_EQ(images.size(), 1);
|
||||
images_file_ << t << std::endl;
|
||||
void AdaptiveSamplingBenchmarkPublisher::imageCallback(
|
||||
const ImagePtrVector& images, Time t
|
||||
) {
|
||||
CHECK_EQ(images.size(), 1);
|
||||
images_file_ << t << std::endl;
|
||||
|
||||
ImagePtr img = images[0];
|
||||
cv::Mat img_8bit;
|
||||
img->convertTo(img_8bit, CV_8U, 255);
|
||||
ImagePtr img = images[0];
|
||||
cv::Mat img_8bit;
|
||||
img->convertTo(img_8bit, CV_8U, 255);
|
||||
|
||||
if(image_index_ == 0)
|
||||
{
|
||||
static const std::vector<int> compression_params = {cv::IMWRITE_PNG_COMPRESSION, 0};
|
||||
if (image_index_ == 0) {
|
||||
static const std::vector<int> compression_params = {
|
||||
cv::IMWRITE_PNG_COMPRESSION,
|
||||
0};
|
||||
|
||||
std::stringstream ss;
|
||||
ss << ze::joinPath(FLAGS_adaptive_sampling_benchmark_folder, "image_");
|
||||
ss << image_index_ << ".png";
|
||||
std::stringstream ss;
|
||||
ss << ze::joinPath(
|
||||
FLAGS_adaptive_sampling_benchmark_folder,
|
||||
"image_"
|
||||
);
|
||||
ss << image_index_ << ".png";
|
||||
|
||||
LOG(INFO) << ss.str();
|
||||
cv::imwrite(ss.str(), img_8bit, compression_params);
|
||||
}
|
||||
LOG(INFO) << ss.str();
|
||||
cv::imwrite(ss.str(), img_8bit, compression_params);
|
||||
}
|
||||
|
||||
for(const PixelLocation& pixel_loc : pixels_to_record_)
|
||||
{
|
||||
// write line in the form "x y I(x,y)"
|
||||
const int x = pixel_loc.first;
|
||||
const int y = pixel_loc.second;
|
||||
pixel_intensities_file_ << x << " "
|
||||
<< y << " "
|
||||
<< (*images[0])(y,x) << std::endl;
|
||||
}
|
||||
for (const PixelLocation& pixel_loc : pixels_to_record_) {
|
||||
// write line in the form "x y I(x,y)"
|
||||
const int x = pixel_loc.first;
|
||||
const int y = pixel_loc.second;
|
||||
pixel_intensities_file_ << x << " " << y << " "
|
||||
<< (*images[0])(y, x) << std::endl;
|
||||
}
|
||||
|
||||
image_index_++;
|
||||
}
|
||||
image_index_++;
|
||||
}
|
||||
|
||||
void AdaptiveSamplingBenchmarkPublisher::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t)
|
||||
{
|
||||
CHECK_EQ(optic_flows.size(), 1);
|
||||
for(const PixelLocation& pixel_loc : pixels_to_record_)
|
||||
{
|
||||
// write line in the form "x y optic_flow(x,y)[0] optic_flow(x,y)[1]"
|
||||
const int x = pixel_loc.first;
|
||||
const int y = pixel_loc.second;
|
||||
optic_flows_file_ << x << " "
|
||||
<< y << " "
|
||||
<< (*optic_flows[0])(y,x)[0] << " "
|
||||
<< (*optic_flows[0])(y,x)[1]
|
||||
<< std::endl;
|
||||
}
|
||||
}
|
||||
void AdaptiveSamplingBenchmarkPublisher::opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, Time t
|
||||
) {
|
||||
CHECK_EQ(optic_flows.size(), 1);
|
||||
for (const PixelLocation& pixel_loc : pixels_to_record_) {
|
||||
// write line in the form "x y optic_flow(x,y)[0]
|
||||
// optic_flow(x,y)[1]"
|
||||
const int x = pixel_loc.first;
|
||||
const int y = pixel_loc.second;
|
||||
optic_flows_file_ << x << " " << y << " "
|
||||
<< (*optic_flows[0])(y, x)[0] << " "
|
||||
<< (*optic_flows[0])(y, x)[1] << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void AdaptiveSamplingBenchmarkPublisher::eventsCallback(
|
||||
const EventsVector& events
|
||||
) {
|
||||
CHECK_EQ(events.size(), 1);
|
||||
|
||||
void AdaptiveSamplingBenchmarkPublisher::eventsCallback(const EventsVector& events)
|
||||
{
|
||||
CHECK_EQ(events.size(), 1);
|
||||
|
||||
for(const Event& e : events[0])
|
||||
{
|
||||
events_file_ << e.t << " " << e.x << " " << e.y << " " << (e.pol? 1 : 0) << std::endl;
|
||||
}
|
||||
}
|
||||
for (const Event& e : events[0]) {
|
||||
events_file_ << e.t << " " << e.x << " " << e.y << " "
|
||||
<< (e.pol ? 1 : 0) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -1,400 +1,417 @@
|
||||
#include <esim/visualization/ros_publisher.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
#include <esim/visualization/ros_publisher.hpp>
|
||||
#include <esim/visualization/ros_utils.hpp>
|
||||
#include <minkindr_conversions/kindr_msg.h>
|
||||
#include <minkindr_conversions/kindr_tf.h>
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <minkindr_conversions/kindr_msg.h>
|
||||
#include <minkindr_conversions/kindr_tf.h>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
|
||||
DEFINE_double(ros_publisher_camera_info_rate, 0,
|
||||
"Camera info (maximum) publish rate, in Hz");
|
||||
DEFINE_double(
|
||||
ros_publisher_camera_info_rate,
|
||||
0,
|
||||
"Camera info (maximum) publish rate, in Hz"
|
||||
);
|
||||
|
||||
DEFINE_double(ros_publisher_frame_rate, 30,
|
||||
"(Maximum) frame rate, in Hz");
|
||||
DEFINE_double(ros_publisher_frame_rate, 30, "(Maximum) frame rate, in Hz");
|
||||
|
||||
DEFINE_double(ros_publisher_depth_rate, 0,
|
||||
"(Maximum) depthmap publish rate, in Hz");
|
||||
DEFINE_double(
|
||||
ros_publisher_depth_rate, 0, "(Maximum) depthmap publish rate, in Hz"
|
||||
);
|
||||
|
||||
DEFINE_double(ros_publisher_pointcloud_rate, 0,
|
||||
"(Maximum) point cloud publish rate, in Hz");
|
||||
DEFINE_double(
|
||||
ros_publisher_pointcloud_rate,
|
||||
0,
|
||||
"(Maximum) point cloud publish rate, in Hz"
|
||||
);
|
||||
|
||||
DEFINE_double(ros_publisher_optic_flow_rate, 0,
|
||||
"(Maximum) optic flow map publish rate, in Hz");
|
||||
DEFINE_double(
|
||||
ros_publisher_optic_flow_rate,
|
||||
0,
|
||||
"(Maximum) optic flow map publish rate, in Hz"
|
||||
);
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
RosPublisher::RosPublisher(size_t num_cameras)
|
||||
{
|
||||
CHECK_GE(num_cameras, 1);
|
||||
num_cameras_ = num_cameras;
|
||||
sensor_sizes_ = std::vector<cv::Size>(num_cameras_);
|
||||
RosPublisher::RosPublisher(size_t num_cameras) {
|
||||
CHECK_GE(num_cameras, 1);
|
||||
num_cameras_ = num_cameras;
|
||||
sensor_sizes_ = std::vector<cv::Size>(num_cameras_);
|
||||
|
||||
// Initialize ROS if it was not initialized before.
|
||||
if(!ros::isInitialized())
|
||||
{
|
||||
VLOG(1) << "Initializing ROS";
|
||||
int argc = 0;
|
||||
ros::init(argc, nullptr, std::string("ros_publisher"), ros::init_options::NoSigintHandler);
|
||||
}
|
||||
// Initialize ROS if it was not initialized before.
|
||||
if (!ros::isInitialized()) {
|
||||
VLOG(1) << "Initializing ROS";
|
||||
int argc = 0;
|
||||
ros::init(
|
||||
argc,
|
||||
nullptr,
|
||||
std::string("ros_publisher"),
|
||||
ros::init_options::NoSigintHandler
|
||||
);
|
||||
}
|
||||
|
||||
// Create node and subscribe.
|
||||
nh_.reset(new ros::NodeHandle(""));
|
||||
it_.reset(new image_transport::ImageTransport(*nh_));
|
||||
// Create node and subscribe.
|
||||
nh_.reset(new ros::NodeHandle(""));
|
||||
it_.reset(new image_transport::ImageTransport(*nh_));
|
||||
|
||||
// Setup ROS publishers for images, events, poses, depth maps, camera info, etc.
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
event_pub_.emplace_back(
|
||||
new ros::Publisher(
|
||||
nh_->advertise<dvs_msgs::EventArray> (getTopicName(i, "events"), 0)));
|
||||
// Setup ROS publishers for images, events, poses, depth maps, camera
|
||||
// info, etc.
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
event_pub_.emplace_back(
|
||||
new ros::Publisher(nh_->advertise<dvs_msgs::EventArray>(
|
||||
getTopicName(i, "events"),
|
||||
0
|
||||
))
|
||||
);
|
||||
|
||||
image_pub_.emplace_back(
|
||||
new image_transport::Publisher(
|
||||
it_->advertise(getTopicName(i, "image_raw"), 0)));
|
||||
image_pub_.emplace_back(new image_transport::Publisher(
|
||||
it_->advertise(getTopicName(i, "image_raw"), 0)
|
||||
));
|
||||
|
||||
image_corrupted_pub_.emplace_back(
|
||||
new image_transport::Publisher(
|
||||
it_->advertise(getTopicName(i, "image_corrupted"), 0)));
|
||||
image_corrupted_pub_.emplace_back(new image_transport::Publisher(
|
||||
it_->advertise(getTopicName(i, "image_corrupted"), 0)
|
||||
));
|
||||
|
||||
depthmap_pub_.emplace_back(
|
||||
new image_transport::Publisher(
|
||||
it_->advertise(getTopicName(i, "depthmap"), 0)));
|
||||
depthmap_pub_.emplace_back(new image_transport::Publisher(
|
||||
it_->advertise(getTopicName(i, "depthmap"), 0)
|
||||
));
|
||||
|
||||
optic_flow_pub_.emplace_back(
|
||||
new ros::Publisher(
|
||||
nh_->advertise<esim_msgs::OpticFlow> (getTopicName(i, "optic_flow"), 0)));
|
||||
optic_flow_pub_.emplace_back(
|
||||
new ros::Publisher(nh_->advertise<esim_msgs::OpticFlow>(
|
||||
getTopicName(i, "optic_flow"),
|
||||
0
|
||||
))
|
||||
);
|
||||
|
||||
camera_info_pub_.emplace_back(
|
||||
new ros::Publisher(
|
||||
nh_->advertise<sensor_msgs::CameraInfo> (getTopicName(i, "camera_info"), 0)));
|
||||
camera_info_pub_.emplace_back(
|
||||
new ros::Publisher(nh_->advertise<sensor_msgs::CameraInfo>(
|
||||
getTopicName(i, "camera_info"),
|
||||
0
|
||||
))
|
||||
);
|
||||
|
||||
twist_pub_.emplace_back(
|
||||
new ros::Publisher(
|
||||
nh_->advertise<geometry_msgs::TwistStamped> (getTopicName(i, "twist"), 0)));
|
||||
twist_pub_.emplace_back(
|
||||
new ros::Publisher(nh_->advertise<geometry_msgs::TwistStamped>(
|
||||
getTopicName(i, "twist"),
|
||||
0
|
||||
))
|
||||
);
|
||||
|
||||
pointcloud_pub_.emplace_back(
|
||||
new ros::Publisher(
|
||||
nh_->advertise<pcl::PointCloud<pcl::PointXYZ>> (getTopicName(i, "pointcloud"), 0)));
|
||||
}
|
||||
pointcloud_pub_.emplace_back(new ros::Publisher(
|
||||
nh_->advertise<pcl::PointCloud<pcl::PointXYZ>>(
|
||||
getTopicName(i, "pointcloud"),
|
||||
0
|
||||
)
|
||||
));
|
||||
}
|
||||
|
||||
pose_pub_.reset(new ros::Publisher(nh_->advertise<geometry_msgs::PoseStamped> ("pose", 0)));
|
||||
imu_pub_.reset(new ros::Publisher(nh_->advertise<sensor_msgs::Imu> ("imu", 0)));
|
||||
tf_broadcaster_.reset(new tf::TransformBroadcaster());
|
||||
pose_pub_.reset(new ros::Publisher(
|
||||
nh_->advertise<geometry_msgs::PoseStamped>("pose", 0)
|
||||
));
|
||||
imu_pub_.reset(
|
||||
new ros::Publisher(nh_->advertise<sensor_msgs::Imu>("imu", 0))
|
||||
);
|
||||
tf_broadcaster_.reset(new tf::TransformBroadcaster());
|
||||
|
||||
last_published_camera_info_time_ = 0;
|
||||
last_published_image_time_ = 0;
|
||||
last_published_corrupted_image_time_ = 0;
|
||||
last_published_depthmap_time_ = 0;
|
||||
last_published_optic_flow_time_ = 0;
|
||||
last_published_pointcloud_time_ = 0;
|
||||
}
|
||||
|
||||
RosPublisher::~RosPublisher()
|
||||
{
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
event_pub_[i]->shutdown();
|
||||
image_pub_[i]->shutdown();
|
||||
image_corrupted_pub_[i]->shutdown();
|
||||
depthmap_pub_[i]->shutdown();
|
||||
optic_flow_pub_[i]->shutdown();
|
||||
camera_info_pub_[i]->shutdown();
|
||||
twist_pub_[i]->shutdown();
|
||||
pointcloud_pub_[i]->shutdown();
|
||||
}
|
||||
pose_pub_->shutdown();
|
||||
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
void RosPublisher::pointcloudCallback(const PointCloudVector& pointclouds, Time t)
|
||||
{
|
||||
CHECK_EQ(pointcloud_pub_.size(), num_cameras_);
|
||||
CHECK_EQ(pointclouds.size(), num_cameras_);
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
const PointCloud& pcl_camera = pointclouds[i];
|
||||
|
||||
CHECK(pointcloud_pub_[i]);
|
||||
if(pointcloud_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
last_published_camera_info_time_ = 0;
|
||||
last_published_image_time_ = 0;
|
||||
last_published_corrupted_image_time_ = 0;
|
||||
last_published_depthmap_time_ = 0;
|
||||
last_published_optic_flow_time_ = 0;
|
||||
last_published_pointcloud_time_ = 0;
|
||||
}
|
||||
|
||||
Duration min_time_interval_between_published_pointclouds_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
|
||||
if(last_published_pointcloud_time_ > 0 && t - last_published_pointcloud_time_ < min_time_interval_between_published_pointclouds_)
|
||||
{
|
||||
return;
|
||||
RosPublisher::~RosPublisher() {
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
event_pub_[i]->shutdown();
|
||||
image_pub_[i]->shutdown();
|
||||
image_corrupted_pub_[i]->shutdown();
|
||||
depthmap_pub_[i]->shutdown();
|
||||
optic_flow_pub_[i]->shutdown();
|
||||
camera_info_pub_[i]->shutdown();
|
||||
twist_pub_[i]->shutdown();
|
||||
pointcloud_pub_[i]->shutdown();
|
||||
}
|
||||
pose_pub_->shutdown();
|
||||
|
||||
ros::shutdown();
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr msg (new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
std::stringstream ss; ss << "cam" << i;
|
||||
pointCloudToMsg(pointclouds[i], ss.str(), t, msg);
|
||||
pointcloud_pub_[i]->publish(msg);
|
||||
}
|
||||
void RosPublisher::pointcloudCallback(
|
||||
const PointCloudVector& pointclouds, Time t
|
||||
) {
|
||||
CHECK_EQ(pointcloud_pub_.size(), num_cameras_);
|
||||
CHECK_EQ(pointclouds.size(), num_cameras_);
|
||||
|
||||
last_published_pointcloud_time_ = t;
|
||||
}
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
const PointCloud& pcl_camera = pointclouds[i];
|
||||
|
||||
void RosPublisher::imageCallback(const ImagePtrVector& images, Time t)
|
||||
{
|
||||
CHECK_EQ(image_pub_.size(), num_cameras_);
|
||||
CHECK(pointcloud_pub_[i]);
|
||||
if (pointcloud_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
sensor_sizes_[i] = images[i]->size();
|
||||
Duration min_time_interval_between_published_pointclouds_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
|
||||
if (last_published_pointcloud_time_ > 0
|
||||
&& t - last_published_pointcloud_time_
|
||||
< min_time_interval_between_published_pointclouds_) {
|
||||
return;
|
||||
}
|
||||
|
||||
CHECK(image_pub_[i]);
|
||||
if(image_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
|
||||
msg(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i;
|
||||
pointCloudToMsg(pointclouds[i], ss.str(), t, msg);
|
||||
pointcloud_pub_[i]->publish(msg);
|
||||
}
|
||||
|
||||
last_published_pointcloud_time_ = t;
|
||||
}
|
||||
|
||||
static const Duration min_time_interval_between_published_images_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if(last_published_image_time_ > 0 && t - last_published_image_time_ < min_time_interval_between_published_images_)
|
||||
{
|
||||
return;
|
||||
void RosPublisher::imageCallback(const ImagePtrVector& images, Time t) {
|
||||
CHECK_EQ(image_pub_.size(), num_cameras_);
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
sensor_sizes_[i] = images[i]->size();
|
||||
|
||||
CHECK(image_pub_[i]);
|
||||
if (image_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
static const Duration min_time_interval_between_published_images_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if (last_published_image_time_ > 0
|
||||
&& t - last_published_image_time_
|
||||
< min_time_interval_between_published_images_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (images[i]) {
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*images[i], t, msg);
|
||||
image_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
last_published_image_time_ = t;
|
||||
}
|
||||
|
||||
if(images[i])
|
||||
{
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*images[i], t, msg);
|
||||
image_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
void RosPublisher::imageCorruptedCallback(
|
||||
const ImagePtrVector& corrupted_images, Time t
|
||||
) {
|
||||
CHECK_EQ(image_corrupted_pub_.size(), num_cameras_);
|
||||
|
||||
last_published_image_time_ = t;
|
||||
}
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
CHECK(image_corrupted_pub_[i]);
|
||||
if (image_corrupted_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
void RosPublisher::imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t)
|
||||
{
|
||||
CHECK_EQ(image_corrupted_pub_.size(), num_cameras_);
|
||||
static const Duration min_time_interval_between_published_images_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if (last_published_corrupted_image_time_ > 0
|
||||
&& t - last_published_corrupted_image_time_
|
||||
< min_time_interval_between_published_images_) {
|
||||
return;
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
CHECK(image_corrupted_pub_[i]);
|
||||
if(image_corrupted_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
if (corrupted_images[i]) {
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*corrupted_images[i], t, msg);
|
||||
image_corrupted_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
last_published_corrupted_image_time_ = t;
|
||||
}
|
||||
|
||||
static const Duration min_time_interval_between_published_images_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if(last_published_corrupted_image_time_ > 0 && t - last_published_corrupted_image_time_ < min_time_interval_between_published_images_)
|
||||
{
|
||||
return;
|
||||
void
|
||||
RosPublisher::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {
|
||||
CHECK_EQ(depthmap_pub_.size(), num_cameras_);
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
CHECK(depthmap_pub_[i]);
|
||||
if (depthmap_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
static const Duration
|
||||
min_time_interval_between_published_depthmaps_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
|
||||
if (last_published_depthmap_time_ > 0
|
||||
&& t - last_published_depthmap_time_
|
||||
< min_time_interval_between_published_depthmaps_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (depthmaps[i]) {
|
||||
sensor_msgs::ImagePtr msg;
|
||||
depthmapToMsg(*depthmaps[i], t, msg);
|
||||
depthmap_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
last_published_depthmap_time_ = t;
|
||||
}
|
||||
|
||||
if(corrupted_images[i])
|
||||
{
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*corrupted_images[i], t, msg);
|
||||
image_corrupted_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
void RosPublisher::opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, Time t
|
||||
) {
|
||||
CHECK_EQ(optic_flow_pub_.size(), num_cameras_);
|
||||
|
||||
last_published_corrupted_image_time_ = t;
|
||||
}
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
CHECK(optic_flow_pub_[i]);
|
||||
if (optic_flow_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
void RosPublisher::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t)
|
||||
{
|
||||
CHECK_EQ(depthmap_pub_.size(), num_cameras_);
|
||||
static const Duration
|
||||
min_time_interval_between_published_optic_flows_ =
|
||||
(min_time_interval_between_published_optic_flows_ > 0)
|
||||
? ze::secToNanosec(
|
||||
1.0 / FLAGS_ros_publisher_optic_flow_rate
|
||||
)
|
||||
: 0;
|
||||
if (min_time_interval_between_published_optic_flows_ > 0
|
||||
&& last_published_optic_flow_time_ > 0
|
||||
&& t - last_published_optic_flow_time_
|
||||
< min_time_interval_between_published_optic_flows_) {
|
||||
return;
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
CHECK(depthmap_pub_[i]);
|
||||
if(depthmap_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
if (optic_flows[i]) {
|
||||
esim_msgs::OpticFlow::Ptr msg;
|
||||
msg.reset(new esim_msgs::OpticFlow);
|
||||
opticFlowToMsg(*optic_flows[i], t, msg);
|
||||
optic_flow_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
last_published_optic_flow_time_ = t;
|
||||
}
|
||||
|
||||
static const Duration min_time_interval_between_published_depthmaps_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
|
||||
if(last_published_depthmap_time_ > 0 && t - last_published_depthmap_time_ < min_time_interval_between_published_depthmaps_)
|
||||
{
|
||||
return;
|
||||
void RosPublisher::eventsCallback(const EventsVector& events) {
|
||||
CHECK_EQ(event_pub_.size(), num_cameras_);
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
if (sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
|
||||
continue;
|
||||
|
||||
if (events[i].empty())
|
||||
continue;
|
||||
|
||||
CHECK(event_pub_[i]);
|
||||
if (event_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
dvs_msgs::EventArrayPtr msg;
|
||||
msg.reset(new dvs_msgs::EventArray);
|
||||
eventsToMsg(
|
||||
events[i],
|
||||
sensor_sizes_[i].width,
|
||||
sensor_sizes_[i].height,
|
||||
msg
|
||||
);
|
||||
event_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
if(depthmaps[i])
|
||||
{
|
||||
sensor_msgs::ImagePtr msg;
|
||||
depthmapToMsg(*depthmaps[i], t, msg);
|
||||
depthmap_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
void RosPublisher::poseCallback(
|
||||
const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t
|
||||
) {
|
||||
if (T_W_Cs.size() != num_cameras_) {
|
||||
LOG(WARNING
|
||||
) << "Number of poses is different than number of cameras."
|
||||
<< "Will not output poses.";
|
||||
return;
|
||||
}
|
||||
|
||||
last_published_depthmap_time_ = t;
|
||||
}
|
||||
// Publish to tf
|
||||
tf::StampedTransform bt;
|
||||
bt.child_frame_id_ = "body";
|
||||
bt.frame_id_ = "map";
|
||||
bt.stamp_ = toRosTime(t);
|
||||
tf::transformKindrToTF(T_W_B, &bt);
|
||||
tf_broadcaster_->sendTransform(bt);
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i;
|
||||
tf::StampedTransform bt;
|
||||
bt.child_frame_id_ = ss.str();
|
||||
bt.frame_id_ = "map";
|
||||
bt.stamp_ = toRosTime(t);
|
||||
tf::transformKindrToTF(T_W_Cs[i], &bt);
|
||||
tf_broadcaster_->sendTransform(bt);
|
||||
}
|
||||
|
||||
void RosPublisher::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t)
|
||||
{
|
||||
CHECK_EQ(optic_flow_pub_.size(), num_cameras_);
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
CHECK(optic_flow_pub_[i]);
|
||||
if(optic_flow_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
// Publish pose message
|
||||
geometry_msgs::PoseStamped pose_stamped_msg;
|
||||
tf::poseStampedKindrToMsg(
|
||||
T_W_B,
|
||||
toRosTime(t),
|
||||
"map",
|
||||
&pose_stamped_msg
|
||||
);
|
||||
pose_pub_->publish(pose_stamped_msg);
|
||||
}
|
||||
|
||||
static const Duration min_time_interval_between_published_optic_flows_
|
||||
= (min_time_interval_between_published_optic_flows_ > 0) ? ze::secToNanosec(1.0 / FLAGS_ros_publisher_optic_flow_rate) : 0;
|
||||
if(min_time_interval_between_published_optic_flows_ > 0 && last_published_optic_flow_time_ > 0 && t - last_published_optic_flow_time_ < min_time_interval_between_published_optic_flows_)
|
||||
{
|
||||
return;
|
||||
void RosPublisher::twistCallback(
|
||||
const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t
|
||||
) {
|
||||
if (ws.size() != num_cameras_ || vs.size() != num_cameras_) {
|
||||
LOG(WARNING
|
||||
) << "Number of twists is different than number of cameras."
|
||||
<< "Will not output twists.";
|
||||
return;
|
||||
}
|
||||
CHECK_EQ(ws.size(), num_cameras_);
|
||||
CHECK_EQ(vs.size(), num_cameras_);
|
||||
CHECK_EQ(twist_pub_.size(), num_cameras_);
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
CHECK(twist_pub_[i]);
|
||||
if (twist_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
const geometry_msgs::TwistStamped msg = twistToMsg(ws[i], vs[i], t);
|
||||
twist_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
if(optic_flows[i])
|
||||
{
|
||||
esim_msgs::OpticFlow::Ptr msg;
|
||||
msg.reset(new esim_msgs::OpticFlow);
|
||||
opticFlowToMsg(*optic_flows[i], t, msg);
|
||||
optic_flow_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
void
|
||||
RosPublisher::imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {
|
||||
if (imu_pub_->getNumSubscribers() == 0)
|
||||
return;
|
||||
|
||||
last_published_optic_flow_time_ = t;
|
||||
}
|
||||
|
||||
void RosPublisher::eventsCallback(const EventsVector& events)
|
||||
{
|
||||
CHECK_EQ(event_pub_.size(), num_cameras_);
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
if(sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
|
||||
{
|
||||
continue;
|
||||
const sensor_msgs::Imu msg = imuToMsg(acc, gyr, t);
|
||||
imu_pub_->publish(msg);
|
||||
}
|
||||
|
||||
if(events[i].empty())
|
||||
{
|
||||
continue;
|
||||
void RosPublisher::cameraInfoCallback(
|
||||
const ze::CameraRig::Ptr& camera_rig, Time t
|
||||
) {
|
||||
CHECK(camera_rig);
|
||||
CHECK_EQ(camera_rig->size(), num_cameras_);
|
||||
|
||||
static const Duration min_time_interval_between_published_camera_info_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
|
||||
if (last_published_camera_info_time_ > 0
|
||||
&& t - last_published_camera_info_time_
|
||||
< min_time_interval_between_published_camera_info_) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
CHECK(camera_info_pub_[i]);
|
||||
if (camera_info_pub_[i]->getNumSubscribers() == 0)
|
||||
continue;
|
||||
|
||||
sensor_msgs::CameraInfoPtr msg;
|
||||
msg.reset(new sensor_msgs::CameraInfo);
|
||||
cameraToMsg(camera_rig->atShared(i), t, msg);
|
||||
camera_info_pub_[i]->publish(msg);
|
||||
}
|
||||
|
||||
last_published_camera_info_time_ = t;
|
||||
}
|
||||
|
||||
CHECK(event_pub_[i]);
|
||||
if(event_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
dvs_msgs::EventArrayPtr msg;
|
||||
msg.reset(new dvs_msgs::EventArray);
|
||||
eventsToMsg(events[i], sensor_sizes_[i].width, sensor_sizes_[i].height, msg);
|
||||
event_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
void RosPublisher::poseCallback(const Transformation& T_W_B,
|
||||
const TransformationVector& T_W_Cs,
|
||||
Time t)
|
||||
{
|
||||
if(T_W_Cs.size() != num_cameras_)
|
||||
{
|
||||
LOG(WARNING) << "Number of poses is different than number of cameras."
|
||||
<< "Will not output poses.";
|
||||
return;
|
||||
}
|
||||
|
||||
// Publish to tf
|
||||
tf::StampedTransform bt;
|
||||
bt.child_frame_id_ = "body";
|
||||
bt.frame_id_ = "map";
|
||||
bt.stamp_ = toRosTime(t);
|
||||
tf::transformKindrToTF(T_W_B, &bt);
|
||||
tf_broadcaster_->sendTransform(bt);
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i;
|
||||
tf::StampedTransform bt;
|
||||
bt.child_frame_id_ = ss.str();
|
||||
bt.frame_id_ = "map";
|
||||
bt.stamp_ = toRosTime(t);
|
||||
tf::transformKindrToTF(T_W_Cs[i], &bt);
|
||||
tf_broadcaster_->sendTransform(bt);
|
||||
}
|
||||
|
||||
// Publish pose message
|
||||
geometry_msgs::PoseStamped pose_stamped_msg;
|
||||
tf::poseStampedKindrToMsg(T_W_B, toRosTime(t), "map", &pose_stamped_msg);
|
||||
pose_pub_->publish(pose_stamped_msg);
|
||||
}
|
||||
|
||||
void RosPublisher::twistCallback(const AngularVelocityVector &ws, const LinearVelocityVector &vs, Time t)
|
||||
{
|
||||
if(ws.size() != num_cameras_
|
||||
|| vs.size() != num_cameras_)
|
||||
{
|
||||
LOG(WARNING) << "Number of twists is different than number of cameras."
|
||||
<< "Will not output twists.";
|
||||
return;
|
||||
}
|
||||
CHECK_EQ(ws.size(), num_cameras_);
|
||||
CHECK_EQ(vs.size(), num_cameras_);
|
||||
CHECK_EQ(twist_pub_.size(), num_cameras_);
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
CHECK(twist_pub_[i]);
|
||||
if(twist_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
const geometry_msgs::TwistStamped msg = twistToMsg(ws[i], vs[i], t);
|
||||
twist_pub_[i]->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
void RosPublisher::imuCallback(const Vector3& acc, const Vector3& gyr, Time t)
|
||||
{
|
||||
if(imu_pub_->getNumSubscribers() == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const sensor_msgs::Imu msg = imuToMsg(acc, gyr, t);
|
||||
imu_pub_->publish(msg);
|
||||
}
|
||||
|
||||
void RosPublisher::cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t)
|
||||
{
|
||||
CHECK(camera_rig);
|
||||
CHECK_EQ(camera_rig->size(), num_cameras_);
|
||||
|
||||
static const Duration min_time_interval_between_published_camera_info_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
|
||||
if(last_published_camera_info_time_ > 0 && t - last_published_camera_info_time_ < min_time_interval_between_published_camera_info_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
CHECK(camera_info_pub_[i]);
|
||||
if(camera_info_pub_[i]->getNumSubscribers() == 0)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
sensor_msgs::CameraInfoPtr msg;
|
||||
msg.reset(new sensor_msgs::CameraInfo);
|
||||
cameraToMsg(camera_rig->atShared(i), t, msg);
|
||||
camera_info_pub_[i]->publish(msg);
|
||||
}
|
||||
|
||||
last_published_camera_info_time_ = t;
|
||||
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -1,174 +1,169 @@
|
||||
#include <esim/visualization/ros_utils.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <cv_bridge/cv_bridge.h>
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <esim/visualization/ros_utils.hpp>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
void pointCloudToMsg(const PointCloud& pointcloud, const std::string& frame_id, Time t, pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg)
|
||||
{
|
||||
CHECK(msg);
|
||||
msg->header.frame_id = frame_id;
|
||||
msg->height = pointcloud.size();
|
||||
msg->width = 1;
|
||||
for(auto& p_c : pointcloud)
|
||||
{
|
||||
pcl::PointXYZRGB p;
|
||||
p.x = p_c.xyz(0);
|
||||
p.y = p_c.xyz(1);
|
||||
p.z = p_c.xyz(2);
|
||||
p.r = p_c.rgb(0);
|
||||
p.g = p_c.rgb(1);
|
||||
p.b = p_c.rgb(2);
|
||||
msg->points.push_back(p);
|
||||
}
|
||||
void pointCloudToMsg(
|
||||
const PointCloud& pointcloud,
|
||||
const std::string& frame_id,
|
||||
Time t,
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr& msg
|
||||
) {
|
||||
CHECK(msg);
|
||||
msg->header.frame_id = frame_id;
|
||||
msg->height = pointcloud.size();
|
||||
msg->width = 1;
|
||||
for (auto& p_c : pointcloud) {
|
||||
pcl::PointXYZRGB p;
|
||||
p.x = p_c.xyz(0);
|
||||
p.y = p_c.xyz(1);
|
||||
p.z = p_c.xyz(2);
|
||||
p.r = p_c.rgb(0);
|
||||
p.g = p_c.rgb(1);
|
||||
p.b = p_c.rgb(2);
|
||||
msg->points.push_back(p);
|
||||
}
|
||||
|
||||
pcl_conversions::toPCL(toRosTime(t), msg->header.stamp);
|
||||
}
|
||||
|
||||
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg)
|
||||
{
|
||||
cv_bridge::CvImage cv_image;
|
||||
image.convertTo(cv_image.image, CV_8U, 255.0);
|
||||
cv_image.encoding = "mono8";
|
||||
cv_image.header.stamp = toRosTime(t);
|
||||
msg = cv_image.toImageMsg();
|
||||
}
|
||||
|
||||
void depthmapToMsg(const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg)
|
||||
{
|
||||
cv_bridge::CvImage cv_depthmap;
|
||||
depthmap.convertTo(cv_depthmap.image, CV_32FC1);
|
||||
cv_depthmap.encoding = "32FC1";
|
||||
cv_depthmap.header.stamp = toRosTime(t);
|
||||
msg = cv_depthmap.toImageMsg();
|
||||
}
|
||||
|
||||
void opticFlowToMsg(const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg)
|
||||
{
|
||||
CHECK(msg);
|
||||
msg->header.stamp = toRosTime(t);
|
||||
|
||||
const int height = flow.rows;
|
||||
const int width = flow.cols;
|
||||
msg->height = height;
|
||||
msg->width = width;
|
||||
|
||||
msg->flow_x.resize(height * width);
|
||||
msg->flow_y.resize(height * width);
|
||||
for(int y=0; y<height; ++y)
|
||||
{
|
||||
for(int x=0; x<width; ++x)
|
||||
{
|
||||
msg->flow_x[x + y * width] = static_cast<float>(flow(y,x)[0]);
|
||||
msg->flow_y[x + y * width] = static_cast<float>(flow(y,x)[1]);
|
||||
pcl_conversions::toPCL(toRosTime(t), msg->header.stamp);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void eventsToMsg(const Events& events, int width, int height, dvs_msgs::EventArrayPtr& msg)
|
||||
{
|
||||
CHECK(msg);
|
||||
std::vector<dvs_msgs::Event> events_list;
|
||||
for(const Event& e : events)
|
||||
{
|
||||
dvs_msgs::Event ev;
|
||||
ev.x = e.x;
|
||||
ev.y = e.y;
|
||||
ev.ts = toRosTime(e.t);
|
||||
ev.polarity = e.pol;
|
||||
|
||||
events_list.push_back(ev);
|
||||
}
|
||||
|
||||
msg->events = events_list;
|
||||
msg->height = height;
|
||||
msg->width = width;
|
||||
msg->header.stamp = events_list.back().ts;
|
||||
}
|
||||
|
||||
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t)
|
||||
{
|
||||
sensor_msgs::Imu imu;
|
||||
imu.header.stamp = toRosTime(t);
|
||||
|
||||
imu.linear_acceleration.x = acc(0);
|
||||
imu.linear_acceleration.y = acc(1);
|
||||
imu.linear_acceleration.z = acc(2);
|
||||
|
||||
imu.angular_velocity.x = gyr(0);
|
||||
imu.angular_velocity.y = gyr(1);
|
||||
imu.angular_velocity.z = gyr(2);
|
||||
|
||||
return imu;
|
||||
}
|
||||
|
||||
geometry_msgs::TwistStamped twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t)
|
||||
{
|
||||
geometry_msgs::TwistStamped twist;
|
||||
twist.header.stamp = toRosTime(t);
|
||||
|
||||
twist.twist.angular.x = w(0);
|
||||
twist.twist.angular.y = w(1);
|
||||
twist.twist.angular.z = w(2);
|
||||
|
||||
twist.twist.linear.x = v(0);
|
||||
twist.twist.linear.y = v(1);
|
||||
twist.twist.linear.z = v(2);
|
||||
|
||||
return twist;
|
||||
}
|
||||
|
||||
void cameraToMsg(const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg)
|
||||
{
|
||||
CHECK(msg);
|
||||
msg->width = camera->width();
|
||||
msg->height = camera->height();
|
||||
msg->header.stamp = toRosTime(t);
|
||||
|
||||
CalibrationMatrix K = calibrationMatrixFromCamera(camera);
|
||||
boost::array<double, 9> K_vec;
|
||||
std::vector<double> D_vec;
|
||||
for(int i=0; i<3; ++i)
|
||||
{
|
||||
for(int j=0; j<3; ++j)
|
||||
{
|
||||
K_vec[j+i*3] = static_cast<double>(K(i,j));
|
||||
void imageToMsg(const Image& image, Time t, sensor_msgs::ImagePtr& msg) {
|
||||
cv_bridge::CvImage cv_image;
|
||||
image.convertTo(cv_image.image, CV_8U, 255.0);
|
||||
cv_image.encoding = "mono8";
|
||||
cv_image.header.stamp = toRosTime(t);
|
||||
msg = cv_image.toImageMsg();
|
||||
}
|
||||
}
|
||||
|
||||
switch(camera->type())
|
||||
{
|
||||
case ze::CameraType::PinholeRadialTangential:
|
||||
case ze::CameraType::Pinhole:
|
||||
msg->distortion_model = "plumb_bob";
|
||||
break;
|
||||
case ze::CameraType::PinholeEquidistant:
|
||||
msg->distortion_model = "equidistant";
|
||||
break;
|
||||
case ze::CameraType::PinholeFov:
|
||||
msg->distortion_model = "fov";
|
||||
break;
|
||||
default:
|
||||
LOG(WARNING) << "Unknown camera distortion model";
|
||||
msg->distortion_model = "";
|
||||
break;
|
||||
}
|
||||
void depthmapToMsg(
|
||||
const Depthmap& depthmap, Time t, sensor_msgs::ImagePtr& msg
|
||||
) {
|
||||
cv_bridge::CvImage cv_depthmap;
|
||||
depthmap.convertTo(cv_depthmap.image, CV_32FC1);
|
||||
cv_depthmap.encoding = "32FC1";
|
||||
cv_depthmap.header.stamp = toRosTime(t);
|
||||
msg = cv_depthmap.toImageMsg();
|
||||
}
|
||||
|
||||
for(int j=0; j<camera->distortionParameters().rows(); ++j)
|
||||
{
|
||||
D_vec.push_back(static_cast<double>(camera->distortionParameters()(j))); // @TODO: use the distortion params from the camera
|
||||
}
|
||||
void opticFlowToMsg(
|
||||
const OpticFlow& flow, Time t, esim_msgs::OpticFlowPtr& msg
|
||||
) {
|
||||
CHECK(msg);
|
||||
msg->header.stamp = toRosTime(t);
|
||||
|
||||
msg->K = K_vec;
|
||||
msg->D = D_vec;
|
||||
msg->P = {K(0,0), 0, K(0,2), 0,
|
||||
0, K(1,1), K(1,2), 0,
|
||||
0, 0, 1, 0};
|
||||
msg->R = {1, 0, 0,
|
||||
0, 1, 0,
|
||||
0, 0, 1};
|
||||
}
|
||||
const int height = flow.rows;
|
||||
const int width = flow.cols;
|
||||
msg->height = height;
|
||||
msg->width = width;
|
||||
|
||||
msg->flow_x.resize(height * width);
|
||||
msg->flow_y.resize(height * width);
|
||||
for (int y = 0; y < height; ++y) {
|
||||
for (int x = 0; x < width; ++x) {
|
||||
msg->flow_x[x + y * width] = static_cast<float>(flow(y, x)[0]);
|
||||
msg->flow_y[x + y * width] = static_cast<float>(flow(y, x)[1]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void eventsToMsg(
|
||||
const Events& events,
|
||||
int width,
|
||||
int height,
|
||||
dvs_msgs::EventArrayPtr& msg
|
||||
) {
|
||||
CHECK(msg);
|
||||
std::vector<dvs_msgs::Event> events_list;
|
||||
for (const Event& e : events) {
|
||||
dvs_msgs::Event ev;
|
||||
ev.x = e.x;
|
||||
ev.y = e.y;
|
||||
ev.ts = toRosTime(e.t);
|
||||
ev.polarity = e.pol;
|
||||
|
||||
events_list.push_back(ev);
|
||||
}
|
||||
|
||||
msg->events = events_list;
|
||||
msg->height = height;
|
||||
msg->width = width;
|
||||
msg->header.stamp = events_list.back().ts;
|
||||
}
|
||||
|
||||
sensor_msgs::Imu imuToMsg(const Vector3& acc, const Vector3& gyr, Time t) {
|
||||
sensor_msgs::Imu imu;
|
||||
imu.header.stamp = toRosTime(t);
|
||||
|
||||
imu.linear_acceleration.x = acc(0);
|
||||
imu.linear_acceleration.y = acc(1);
|
||||
imu.linear_acceleration.z = acc(2);
|
||||
|
||||
imu.angular_velocity.x = gyr(0);
|
||||
imu.angular_velocity.y = gyr(1);
|
||||
imu.angular_velocity.z = gyr(2);
|
||||
|
||||
return imu;
|
||||
}
|
||||
|
||||
geometry_msgs::TwistStamped
|
||||
twistToMsg(const AngularVelocity& w, const LinearVelocity& v, Time t) {
|
||||
geometry_msgs::TwistStamped twist;
|
||||
twist.header.stamp = toRosTime(t);
|
||||
|
||||
twist.twist.angular.x = w(0);
|
||||
twist.twist.angular.y = w(1);
|
||||
twist.twist.angular.z = w(2);
|
||||
|
||||
twist.twist.linear.x = v(0);
|
||||
twist.twist.linear.y = v(1);
|
||||
twist.twist.linear.z = v(2);
|
||||
|
||||
return twist;
|
||||
}
|
||||
|
||||
void cameraToMsg(
|
||||
const ze::Camera::Ptr& camera, Time t, sensor_msgs::CameraInfoPtr& msg
|
||||
) {
|
||||
CHECK(msg);
|
||||
msg->width = camera->width();
|
||||
msg->height = camera->height();
|
||||
msg->header.stamp = toRosTime(t);
|
||||
|
||||
CalibrationMatrix K = calibrationMatrixFromCamera(camera);
|
||||
boost::array<double, 9> K_vec;
|
||||
std::vector<double> D_vec;
|
||||
for (int i = 0; i < 3; ++i)
|
||||
for (int j = 0; j < 3; ++j)
|
||||
K_vec[j + i * 3] = static_cast<double>(K(i, j));
|
||||
|
||||
switch (camera->type()) {
|
||||
case ze::CameraType::PinholeRadialTangential:
|
||||
case ze::CameraType::Pinhole:
|
||||
msg->distortion_model = "plumb_bob";
|
||||
break;
|
||||
case ze::CameraType::PinholeEquidistant:
|
||||
msg->distortion_model = "equidistant";
|
||||
break;
|
||||
case ze::CameraType::PinholeFov:
|
||||
msg->distortion_model = "fov";
|
||||
break;
|
||||
default:
|
||||
LOG(WARNING) << "Unknown camera distortion model";
|
||||
msg->distortion_model = "";
|
||||
break;
|
||||
}
|
||||
|
||||
for (int j = 0; j < camera->distortionParameters().rows(); ++j) {
|
||||
D_vec.push_back(static_cast<double>(camera->distortionParameters()(j
|
||||
))); // @TODO: use the distortion params from the camera
|
||||
}
|
||||
|
||||
msg->K = K_vec;
|
||||
msg->D = D_vec;
|
||||
msg->P = {K(0, 0), 0, K(0, 2), 0, 0, K(1, 1), K(1, 2), 0, 0, 0, 1, 0};
|
||||
msg->R = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -1,13 +1,12 @@
|
||||
#include <esim/visualization/rosbag_writer.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
#include <esim/visualization/ros_utils.hpp>
|
||||
#include <esim/visualization/rosbag_writer.hpp>
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <minkindr_conversions/kindr_msg.h>
|
||||
#include <minkindr_conversions/kindr_tf.h>
|
||||
#include <tf/tfMessage.h>
|
||||
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
|
||||
DECLARE_double(ros_publisher_camera_info_rate);
|
||||
DECLARE_double(ros_publisher_frame_rate);
|
||||
@ -15,294 +14,321 @@ DECLARE_double(ros_publisher_depth_rate);
|
||||
DECLARE_double(ros_publisher_pointcloud_rate);
|
||||
DECLARE_double(ros_publisher_optic_flow_rate);
|
||||
|
||||
DEFINE_string(path_to_output_bag, "",
|
||||
"Path to which save the output bag file.");
|
||||
DEFINE_string(
|
||||
path_to_output_bag, "", "Path to which save the output bag file."
|
||||
);
|
||||
|
||||
namespace event_camera_simulator {
|
||||
|
||||
RosbagWriter::RosbagWriter(const std::string& path_to_output_bag, size_t num_cameras)
|
||||
{
|
||||
CHECK_GE(num_cameras, 1);
|
||||
num_cameras_ = num_cameras;
|
||||
sensor_sizes_ = std::vector<cv::Size>(num_cameras_);
|
||||
RosbagWriter::RosbagWriter(
|
||||
const std::string& path_to_output_bag, size_t num_cameras
|
||||
) {
|
||||
CHECK_GE(num_cameras, 1);
|
||||
num_cameras_ = num_cameras;
|
||||
sensor_sizes_ = std::vector<cv::Size>(num_cameras_);
|
||||
|
||||
try
|
||||
{
|
||||
bag_.open(path_to_output_bag, rosbag::bagmode::Write);
|
||||
}
|
||||
catch(rosbag::BagIOException e)
|
||||
{
|
||||
LOG(FATAL) << "Error: could not open rosbag: " << FLAGS_path_to_output_bag << std::endl;
|
||||
return;
|
||||
}
|
||||
try {
|
||||
bag_.open(path_to_output_bag, rosbag::bagmode::Write);
|
||||
} catch (rosbag::BagIOException e) {
|
||||
LOG(FATAL) << "Error: could not open rosbag: "
|
||||
<< FLAGS_path_to_output_bag << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
LOG(INFO) << "Will write to bag: " << path_to_output_bag;
|
||||
LOG(INFO) << "Will write to bag: " << path_to_output_bag;
|
||||
|
||||
last_published_camera_info_time_ = 0;
|
||||
last_published_image_time_ = 0;
|
||||
last_published_corrupted_image_time_ = 0;
|
||||
last_published_depthmap_time_ = 0;
|
||||
last_published_optic_flow_time_ = 0;
|
||||
last_published_pointcloud_time_ = 0;
|
||||
}
|
||||
|
||||
Publisher::Ptr RosbagWriter::createBagWriterFromGflags(size_t num_cameras)
|
||||
{
|
||||
if(FLAGS_path_to_output_bag == "")
|
||||
{
|
||||
LOG(INFO) << "Empty output bag string: will not write to rosbag";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return std::make_shared<RosbagWriter>(FLAGS_path_to_output_bag, num_cameras);
|
||||
}
|
||||
|
||||
RosbagWriter::~RosbagWriter()
|
||||
{
|
||||
LOG(INFO) << "Finalizing the bag...";
|
||||
bag_.close();
|
||||
LOG(INFO) << "Finished writing to bag: " << FLAGS_path_to_output_bag;
|
||||
}
|
||||
|
||||
void RosbagWriter::pointcloudCallback(const PointCloudVector& pointclouds, Time t)
|
||||
{
|
||||
CHECK_EQ(pointclouds.size(), num_cameras_);
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
Duration min_time_interval_between_published_pointclouds_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
|
||||
if(last_published_pointcloud_time_ > 0 && t - last_published_pointcloud_time_ < min_time_interval_between_published_pointclouds_)
|
||||
{
|
||||
return;
|
||||
last_published_camera_info_time_ = 0;
|
||||
last_published_image_time_ = 0;
|
||||
last_published_corrupted_image_time_ = 0;
|
||||
last_published_depthmap_time_ = 0;
|
||||
last_published_optic_flow_time_ = 0;
|
||||
last_published_pointcloud_time_ = 0;
|
||||
}
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr msg (new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
std::stringstream ss; ss << "cam" << i;
|
||||
pointCloudToMsg(pointclouds[i], ss.str(), t, msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "pointcloud"),
|
||||
toRosTime(t), msg);
|
||||
}
|
||||
last_published_pointcloud_time_ = t;
|
||||
}
|
||||
Publisher::Ptr RosbagWriter::createBagWriterFromGflags(size_t num_cameras) {
|
||||
if (FLAGS_path_to_output_bag == "") {
|
||||
LOG(INFO) << "Empty output bag string: will not write to rosbag";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
void RosbagWriter::imageCallback(const ImagePtrVector& images, Time t)
|
||||
{
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
sensor_sizes_[i] = images[i]->size();
|
||||
|
||||
static const Duration min_time_interval_between_published_images_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if(last_published_image_time_ > 0 && t - last_published_image_time_ < min_time_interval_between_published_images_)
|
||||
{
|
||||
return;
|
||||
return std::make_shared<RosbagWriter>(
|
||||
FLAGS_path_to_output_bag,
|
||||
num_cameras
|
||||
);
|
||||
}
|
||||
|
||||
if(images[i])
|
||||
{
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*images[i], t, msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "image_raw"),
|
||||
msg->header.stamp, msg);
|
||||
}
|
||||
}
|
||||
last_published_image_time_ = t;
|
||||
}
|
||||
|
||||
void RosbagWriter::imageCorruptedCallback(const ImagePtrVector& images_corrupted, Time t)
|
||||
{
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
static const Duration min_time_interval_between_published_images_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if(last_published_corrupted_image_time_ > 0 && t - last_published_corrupted_image_time_ < min_time_interval_between_published_images_)
|
||||
{
|
||||
return;
|
||||
RosbagWriter::~RosbagWriter() {
|
||||
LOG(INFO) << "Finalizing the bag...";
|
||||
bag_.close();
|
||||
LOG(INFO) << "Finished writing to bag: " << FLAGS_path_to_output_bag;
|
||||
}
|
||||
|
||||
if(images_corrupted[i])
|
||||
{
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*images_corrupted[i], t, msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "image_corrupted"),
|
||||
msg->header.stamp, msg);
|
||||
}
|
||||
}
|
||||
last_published_corrupted_image_time_ = t;
|
||||
}
|
||||
void RosbagWriter::pointcloudCallback(
|
||||
const PointCloudVector& pointclouds, Time t
|
||||
) {
|
||||
CHECK_EQ(pointclouds.size(), num_cameras_);
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
Duration min_time_interval_between_published_pointclouds_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_pointcloud_rate);
|
||||
if (last_published_pointcloud_time_ > 0
|
||||
&& t - last_published_pointcloud_time_
|
||||
< min_time_interval_between_published_pointclouds_) {
|
||||
return;
|
||||
}
|
||||
|
||||
void RosbagWriter::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t)
|
||||
{
|
||||
if(depthmaps.size() != num_cameras_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
static const Duration min_time_interval_between_published_depthmaps_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
|
||||
if(last_published_depthmap_time_ > 0 && t - last_published_depthmap_time_ < min_time_interval_between_published_depthmaps_)
|
||||
{
|
||||
return;
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
|
||||
msg(new pcl::PointCloud<pcl::PointXYZRGB>);
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i;
|
||||
pointCloudToMsg(pointclouds[i], ss.str(), t, msg);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "pointcloud"),
|
||||
toRosTime(t),
|
||||
msg
|
||||
);
|
||||
}
|
||||
last_published_pointcloud_time_ = t;
|
||||
}
|
||||
|
||||
if(depthmaps[i])
|
||||
{
|
||||
sensor_msgs::ImagePtr msg;
|
||||
depthmapToMsg(*depthmaps[i], t, msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "depthmap"),
|
||||
msg->header.stamp, msg);
|
||||
}
|
||||
}
|
||||
last_published_depthmap_time_ = t;
|
||||
}
|
||||
void RosbagWriter::imageCallback(const ImagePtrVector& images, Time t) {
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
sensor_sizes_[i] = images[i]->size();
|
||||
|
||||
static const Duration min_time_interval_between_published_images_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if (last_published_image_time_ > 0
|
||||
&& t - last_published_image_time_
|
||||
< min_time_interval_between_published_images_) {
|
||||
return;
|
||||
}
|
||||
|
||||
void RosbagWriter::opticFlowCallback(const OpticFlowPtrVector& optic_flows, Time t)
|
||||
{
|
||||
if(optic_flows.size() != num_cameras_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
static const Duration min_time_interval_between_published_optic_flows_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_optic_flow_rate);
|
||||
if(last_published_optic_flow_time_ > 0 && t - last_published_optic_flow_time_ < min_time_interval_between_published_optic_flows_)
|
||||
{
|
||||
return;
|
||||
if (images[i]) {
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*images[i], t, msg);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "image_raw"),
|
||||
msg->header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
}
|
||||
last_published_image_time_ = t;
|
||||
}
|
||||
|
||||
if(optic_flows[i])
|
||||
{
|
||||
esim_msgs::OpticFlow::Ptr msg;
|
||||
msg.reset(new esim_msgs::OpticFlow);
|
||||
opticFlowToMsg(*optic_flows[i], t, msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "optic_flow"),
|
||||
msg->header.stamp, msg);
|
||||
}
|
||||
}
|
||||
void RosbagWriter::imageCorruptedCallback(
|
||||
const ImagePtrVector& images_corrupted, Time t
|
||||
) {
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
static const Duration min_time_interval_between_published_images_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_frame_rate);
|
||||
if (last_published_corrupted_image_time_ > 0
|
||||
&& t - last_published_corrupted_image_time_
|
||||
< min_time_interval_between_published_images_) {
|
||||
return;
|
||||
}
|
||||
|
||||
last_published_optic_flow_time_ = t;
|
||||
}
|
||||
|
||||
void RosbagWriter::eventsCallback(const EventsVector& events)
|
||||
{
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
if(sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
|
||||
{
|
||||
continue;
|
||||
if (images_corrupted[i]) {
|
||||
sensor_msgs::ImagePtr msg;
|
||||
imageToMsg(*images_corrupted[i], t, msg);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "image_corrupted"),
|
||||
msg->header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
}
|
||||
last_published_corrupted_image_time_ = t;
|
||||
}
|
||||
|
||||
if(events[i].empty())
|
||||
{
|
||||
continue;
|
||||
void
|
||||
RosbagWriter::depthmapCallback(const DepthmapPtrVector& depthmaps, Time t) {
|
||||
if (depthmaps.size() != num_cameras_)
|
||||
return;
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
static const Duration
|
||||
min_time_interval_between_published_depthmaps_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_depth_rate);
|
||||
if (last_published_depthmap_time_ > 0
|
||||
&& t - last_published_depthmap_time_
|
||||
< min_time_interval_between_published_depthmaps_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (depthmaps[i]) {
|
||||
sensor_msgs::ImagePtr msg;
|
||||
depthmapToMsg(*depthmaps[i], t, msg);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "depthmap"),
|
||||
msg->header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
}
|
||||
last_published_depthmap_time_ = t;
|
||||
}
|
||||
|
||||
dvs_msgs::EventArrayPtr msg;
|
||||
msg.reset(new dvs_msgs::EventArray);
|
||||
eventsToMsg(events[i], sensor_sizes_[i].width, sensor_sizes_[i].height, msg);
|
||||
void RosbagWriter::opticFlowCallback(
|
||||
const OpticFlowPtrVector& optic_flows, Time t
|
||||
) {
|
||||
if (optic_flows.size() != num_cameras_)
|
||||
return;
|
||||
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "events"),
|
||||
msg->header.stamp, msg);
|
||||
}
|
||||
}
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
static const Duration
|
||||
min_time_interval_between_published_optic_flows_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_optic_flow_rate);
|
||||
if (last_published_optic_flow_time_ > 0
|
||||
&& t - last_published_optic_flow_time_
|
||||
< min_time_interval_between_published_optic_flows_) {
|
||||
return;
|
||||
}
|
||||
|
||||
void RosbagWriter::poseCallback(const Transformation& T_W_B,
|
||||
const TransformationVector& T_W_Cs,
|
||||
Time t)
|
||||
{
|
||||
if(T_W_Cs.size() != num_cameras_)
|
||||
{
|
||||
LOG(WARNING) << "Number of poses is different than number of cameras."
|
||||
<< "Will not output poses.";
|
||||
return;
|
||||
}
|
||||
geometry_msgs::PoseStamped pose_stamped_msg;
|
||||
geometry_msgs::TransformStamped transform_stamped_msg;
|
||||
transform_stamped_msg.header.frame_id = "map";
|
||||
transform_stamped_msg.header.stamp = toRosTime(t);
|
||||
tf::tfMessage tf_msg;
|
||||
if (optic_flows[i]) {
|
||||
esim_msgs::OpticFlow::Ptr msg;
|
||||
msg.reset(new esim_msgs::OpticFlow);
|
||||
opticFlowToMsg(*optic_flows[i], t, msg);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "optic_flow"),
|
||||
msg->header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
// Write pose to bag
|
||||
tf::poseStampedKindrToMsg(T_W_Cs[i], toRosTime(t), "map", &pose_stamped_msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "pose"),
|
||||
toRosTime(t), pose_stamped_msg);
|
||||
last_published_optic_flow_time_ = t;
|
||||
}
|
||||
|
||||
// Write tf transform to bag
|
||||
std::stringstream ss; ss << "cam" << i;
|
||||
transform_stamped_msg.child_frame_id = ss.str();
|
||||
tf::transformKindrToMsg(T_W_Cs[i], &transform_stamped_msg.transform);
|
||||
tf_msg.transforms.push_back(transform_stamped_msg);
|
||||
}
|
||||
void RosbagWriter::eventsCallback(const EventsVector& events) {
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
if (sensor_sizes_[i].width == 0 || sensor_sizes_[i].height == 0)
|
||||
continue;
|
||||
|
||||
transform_stamped_msg.child_frame_id = "body";
|
||||
tf::transformKindrToMsg(T_W_B, &transform_stamped_msg.transform);
|
||||
tf_msg.transforms.push_back(transform_stamped_msg);
|
||||
if (events[i].empty())
|
||||
continue;
|
||||
|
||||
bag_.write("/tf", toRosTime(t), tf_msg);
|
||||
}
|
||||
dvs_msgs::EventArrayPtr msg;
|
||||
msg.reset(new dvs_msgs::EventArray);
|
||||
eventsToMsg(
|
||||
events[i],
|
||||
sensor_sizes_[i].width,
|
||||
sensor_sizes_[i].height,
|
||||
msg
|
||||
);
|
||||
|
||||
void RosbagWriter::twistCallback(const AngularVelocityVector &ws, const LinearVelocityVector &vs, Time t)
|
||||
{
|
||||
if(ws.size() != num_cameras_
|
||||
|| vs.size() != num_cameras_)
|
||||
{
|
||||
LOG(WARNING) << "Number of twists is different than number of cameras."
|
||||
<< "Will not output twists.";
|
||||
return;
|
||||
}
|
||||
CHECK_EQ(ws.size(), num_cameras_);
|
||||
CHECK_EQ(vs.size(), num_cameras_);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "events"),
|
||||
msg->header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
const geometry_msgs::TwistStamped msg = twistToMsg(ws[i], vs[i], t);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "twist"),
|
||||
msg.header.stamp, msg);
|
||||
}
|
||||
}
|
||||
void RosbagWriter::poseCallback(
|
||||
const Transformation& T_W_B, const TransformationVector& T_W_Cs, Time t
|
||||
) {
|
||||
if (T_W_Cs.size() != num_cameras_) {
|
||||
LOG(WARNING
|
||||
) << "Number of poses is different than number of cameras."
|
||||
<< "Will not output poses.";
|
||||
return;
|
||||
}
|
||||
geometry_msgs::PoseStamped pose_stamped_msg;
|
||||
geometry_msgs::TransformStamped transform_stamped_msg;
|
||||
transform_stamped_msg.header.frame_id = "map";
|
||||
transform_stamped_msg.header.stamp = toRosTime(t);
|
||||
tf::tfMessage tf_msg;
|
||||
|
||||
void RosbagWriter::imuCallback(const Vector3& acc, const Vector3& gyr, Time t)
|
||||
{
|
||||
VLOG_EVERY_N(1, 500) << "t = " << ze::nanosecToSecTrunc(t) << " s";
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
// Write pose to bag
|
||||
tf::poseStampedKindrToMsg(
|
||||
T_W_Cs[i],
|
||||
toRosTime(t),
|
||||
"map",
|
||||
&pose_stamped_msg
|
||||
);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "pose"),
|
||||
toRosTime(t),
|
||||
pose_stamped_msg
|
||||
);
|
||||
|
||||
const sensor_msgs::Imu msg = imuToMsg(acc, gyr, t);
|
||||
const std::string imu_topic = "/imu";
|
||||
bag_.write(imu_topic,
|
||||
msg.header.stamp, msg);
|
||||
}
|
||||
// Write tf transform to bag
|
||||
std::stringstream ss;
|
||||
ss << "cam" << i;
|
||||
transform_stamped_msg.child_frame_id = ss.str();
|
||||
tf::transformKindrToMsg(
|
||||
T_W_Cs[i],
|
||||
&transform_stamped_msg.transform
|
||||
);
|
||||
tf_msg.transforms.push_back(transform_stamped_msg);
|
||||
}
|
||||
|
||||
void RosbagWriter::cameraInfoCallback(const ze::CameraRig::Ptr& camera_rig, Time t)
|
||||
{
|
||||
CHECK(camera_rig);
|
||||
CHECK_EQ(camera_rig->size(), num_cameras_);
|
||||
transform_stamped_msg.child_frame_id = "body";
|
||||
tf::transformKindrToMsg(T_W_B, &transform_stamped_msg.transform);
|
||||
tf_msg.transforms.push_back(transform_stamped_msg);
|
||||
|
||||
static const Duration min_time_interval_between_published_camera_info_
|
||||
= ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
|
||||
if(last_published_camera_info_time_ > 0 && t - last_published_camera_info_time_ < min_time_interval_between_published_camera_info_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
bag_.write("/tf", toRosTime(t), tf_msg);
|
||||
}
|
||||
|
||||
for(size_t i=0; i<num_cameras_; ++i)
|
||||
{
|
||||
sensor_msgs::CameraInfoPtr msg;
|
||||
msg.reset(new sensor_msgs::CameraInfo);
|
||||
cameraToMsg(camera_rig->atShared(i), t, msg);
|
||||
bag_.write(getTopicName(topic_name_prefix_, i, "camera_info"),
|
||||
msg->header.stamp, msg);
|
||||
}
|
||||
void RosbagWriter::twistCallback(
|
||||
const AngularVelocityVector& ws, const LinearVelocityVector& vs, Time t
|
||||
) {
|
||||
if (ws.size() != num_cameras_ || vs.size() != num_cameras_) {
|
||||
LOG(WARNING
|
||||
) << "Number of twists is different than number of cameras."
|
||||
<< "Will not output twists.";
|
||||
return;
|
||||
}
|
||||
CHECK_EQ(ws.size(), num_cameras_);
|
||||
CHECK_EQ(vs.size(), num_cameras_);
|
||||
|
||||
last_published_camera_info_time_ = t;
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
const geometry_msgs::TwistStamped msg = twistToMsg(ws[i], vs[i], t);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "twist"),
|
||||
msg.header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
void
|
||||
RosbagWriter::imuCallback(const Vector3& acc, const Vector3& gyr, Time t) {
|
||||
VLOG_EVERY_N(1, 500) << "t = " << ze::nanosecToSecTrunc(t) << " s";
|
||||
|
||||
const sensor_msgs::Imu msg = imuToMsg(acc, gyr, t);
|
||||
const std::string imu_topic = "/imu";
|
||||
bag_.write(imu_topic, msg.header.stamp, msg);
|
||||
}
|
||||
|
||||
void RosbagWriter::cameraInfoCallback(
|
||||
const ze::CameraRig::Ptr& camera_rig, Time t
|
||||
) {
|
||||
CHECK(camera_rig);
|
||||
CHECK_EQ(camera_rig->size(), num_cameras_);
|
||||
|
||||
static const Duration min_time_interval_between_published_camera_info_ =
|
||||
ze::secToNanosec(1.0 / FLAGS_ros_publisher_camera_info_rate);
|
||||
if (last_published_camera_info_time_ > 0
|
||||
&& t - last_published_camera_info_time_
|
||||
< min_time_interval_between_published_camera_info_) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < num_cameras_; ++i) {
|
||||
sensor_msgs::CameraInfoPtr msg;
|
||||
msg.reset(new sensor_msgs::CameraInfo);
|
||||
cameraToMsg(camera_rig->atShared(i), t, msg);
|
||||
bag_.write(
|
||||
getTopicName(topic_name_prefix_, i, "camera_info"),
|
||||
msg->header.stamp,
|
||||
msg
|
||||
);
|
||||
}
|
||||
|
||||
last_published_camera_info_time_ = t;
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
@ -1,107 +1,127 @@
|
||||
#include <esim/visualization/synthetic_optic_flow_publisher.hpp>
|
||||
#include <esim/common/utils.hpp>
|
||||
#include <ze/common/path_utils.hpp>
|
||||
#include <ze/common/file_utils.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
|
||||
#include <esim/visualization/synthetic_optic_flow_publisher.hpp>
|
||||
#include <gflags/gflags.h>
|
||||
#include <glog/logging.h>
|
||||
#include <opencv2/highgui/highgui.hpp>
|
||||
#include <ze/common/file_utils.hpp>
|
||||
#include <ze/common/path_utils.hpp>
|
||||
#include <ze/common/time_conversions.hpp>
|
||||
|
||||
DEFINE_string(synthetic_optic_flow_output_folder, "",
|
||||
"Folder in which to output the events.");
|
||||
DEFINE_string(
|
||||
synthetic_optic_flow_output_folder,
|
||||
"",
|
||||
"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).
|
||||
*/
|
||||
SyntheticOpticFlowPublisher::SyntheticOpticFlowPublisher(const std::string& output_folder)
|
||||
: output_folder_(output_folder)
|
||||
{
|
||||
ze::openOutputFileStream(ze::joinPath(output_folder, "events.txt"),
|
||||
&events_file_);
|
||||
}
|
||||
/**
|
||||
* 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).
|
||||
*/
|
||||
SyntheticOpticFlowPublisher::SyntheticOpticFlowPublisher(
|
||||
const std::string& output_folder
|
||||
)
|
||||
: output_folder_(output_folder) {
|
||||
ze::openOutputFileStream(
|
||||
ze::joinPath(output_folder, "events.txt"),
|
||||
&events_file_
|
||||
);
|
||||
}
|
||||
|
||||
Publisher::Ptr SyntheticOpticFlowPublisher::createFromGflags()
|
||||
{
|
||||
if(FLAGS_synthetic_optic_flow_output_folder == "")
|
||||
{
|
||||
LOG(WARNING) << "Empty output folder string: will not write synthetic optic flow files";
|
||||
return nullptr;
|
||||
}
|
||||
Publisher::Ptr SyntheticOpticFlowPublisher::createFromGflags() {
|
||||
if (FLAGS_synthetic_optic_flow_output_folder == "") {
|
||||
LOG(WARNING
|
||||
) << "Empty output folder string: will not write synthetic "
|
||||
"optic flow files";
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return std::make_shared<SyntheticOpticFlowPublisher>(FLAGS_synthetic_optic_flow_output_folder);
|
||||
}
|
||||
return std::make_shared<SyntheticOpticFlowPublisher>(
|
||||
FLAGS_synthetic_optic_flow_output_folder
|
||||
);
|
||||
}
|
||||
|
||||
SyntheticOpticFlowPublisher::~SyntheticOpticFlowPublisher()
|
||||
{
|
||||
// Create an event count image using all the events collected
|
||||
cv::Mat event_count_image = cv::Mat::zeros(sensor_size_, CV_8UC3);
|
||||
SyntheticOpticFlowPublisher::~SyntheticOpticFlowPublisher() {
|
||||
// Create an event count image using all the events collected
|
||||
cv::Mat event_count_image = cv::Mat::zeros(sensor_size_, CV_8UC3);
|
||||
|
||||
// Create two event timestamps images using all the events collected
|
||||
cv::Mat timestamps_pos = cv::Mat::zeros(sensor_size_, CV_8UC3);
|
||||
cv::Mat timestamps_neg = cv::Mat::zeros(sensor_size_, CV_8UC3);
|
||||
// Create two event timestamps images using all the events collected
|
||||
cv::Mat timestamps_pos = cv::Mat::zeros(sensor_size_, CV_8UC3);
|
||||
cv::Mat timestamps_neg = cv::Mat::zeros(sensor_size_, CV_8UC3);
|
||||
|
||||
int remapped_timestamp_fraction;
|
||||
double timestamp_fraction;
|
||||
for(Event e : events_)
|
||||
{
|
||||
event_count_image.at<cv::Vec3b>(e.y,e.x)[int(e.pol)]++;
|
||||
int remapped_timestamp_fraction;
|
||||
double timestamp_fraction;
|
||||
for (Event e : events_) {
|
||||
event_count_image.at<cv::Vec3b>(e.y, e.x)[int(e.pol)]++;
|
||||
|
||||
cv::Mat& curr_timestamp_image = e.pol ? timestamps_pos : timestamps_neg;
|
||||
cv::Mat& curr_timestamp_image =
|
||||
e.pol ? timestamps_pos : timestamps_neg;
|
||||
|
||||
// remap value
|
||||
timestamp_fraction = double(e.t - events_[0].t) / (events_[events_.size()-1].t - events_[0].t);
|
||||
remapped_timestamp_fraction = timestamp_fraction * std::pow(2,24); // remap 0-1 to 0 - 2^24
|
||||
// remap value
|
||||
timestamp_fraction = double(e.t - events_[0].t)
|
||||
/ (events_[events_.size() - 1].t - events_[0].t);
|
||||
remapped_timestamp_fraction =
|
||||
timestamp_fraction * std::pow(2, 24); // remap 0-1 to 0 - 2^24
|
||||
|
||||
// distribute the 24 bit number (remapped_timestamp_fraction) to 3 channel 8 bit image
|
||||
for (int i=0; i<3; i++)
|
||||
{
|
||||
curr_timestamp_image.at<cv::Vec3b>(e.y,e.x)[i] = (int) remapped_timestamp_fraction & 0xFF; // bit mask of 0000 0000 0000 0000 1111 1111
|
||||
remapped_timestamp_fraction = remapped_timestamp_fraction >> 8; // shifts bits to right by 8
|
||||
}
|
||||
}
|
||||
// distribute the 24 bit number (remapped_timestamp_fraction) to 3
|
||||
// channel 8 bit image
|
||||
for (int i = 0; i < 3; i++) {
|
||||
curr_timestamp_image.at<cv::Vec3b>(e.y, e.x)[i] =
|
||||
(int) remapped_timestamp_fraction
|
||||
& 0xFF; // bit mask of 0000 0000 0000 0000 1111 1111
|
||||
remapped_timestamp_fraction = remapped_timestamp_fraction
|
||||
>> 8; // shifts bits to right by 8
|
||||
}
|
||||
}
|
||||
|
||||
// Write event count image + the two timestamps images to disk
|
||||
std::string path_event_count_image = ze::joinPath(output_folder_, "event_count.png");
|
||||
std::string path_timestamps_pos = ze::joinPath(output_folder_, "event_time_stamps_pos.png");
|
||||
std::string path_timestamps_neg = ze::joinPath(output_folder_, "event_time_stamps_neg.png");
|
||||
// Write event count image + the two timestamps images to disk
|
||||
std::string path_event_count_image =
|
||||
ze::joinPath(output_folder_, "event_count.png");
|
||||
std::string path_timestamps_pos =
|
||||
ze::joinPath(output_folder_, "event_time_stamps_pos.png");
|
||||
std::string path_timestamps_neg =
|
||||
ze::joinPath(output_folder_, "event_time_stamps_neg.png");
|
||||
|
||||
std::vector<int> compression_params;
|
||||
compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION);
|
||||
compression_params.push_back(0);
|
||||
std::vector<int> compression_params;
|
||||
compression_params.push_back(cv::IMWRITE_PNG_COMPRESSION);
|
||||
compression_params.push_back(0);
|
||||
|
||||
cv::imwrite(path_event_count_image, event_count_image, compression_params);
|
||||
cv::imwrite(path_timestamps_pos, timestamps_pos, compression_params);
|
||||
cv::imwrite(path_timestamps_neg, timestamps_neg, compression_params);
|
||||
cv::imwrite(
|
||||
path_event_count_image,
|
||||
event_count_image,
|
||||
compression_params
|
||||
);
|
||||
cv::imwrite(path_timestamps_pos, timestamps_pos, compression_params);
|
||||
cv::imwrite(path_timestamps_neg, timestamps_neg, compression_params);
|
||||
|
||||
// Finish writing event file
|
||||
events_file_.close();
|
||||
}
|
||||
// Finish writing event file
|
||||
events_file_.close();
|
||||
}
|
||||
|
||||
void SyntheticOpticFlowPublisher::eventsCallback(const EventsVector& events)
|
||||
{
|
||||
CHECK_EQ(events.size(), 1);
|
||||
void SyntheticOpticFlowPublisher::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.x << " " << e.y << " " << (e.pol? 1 : 0) << std::endl;
|
||||
events_.push_back(e);
|
||||
}
|
||||
}
|
||||
// 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.x << " " << e.y << " "
|
||||
<< (e.pol ? 1 : 0) << std::endl;
|
||||
events_.push_back(e);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace event_camera_simulator
|
||||
|
||||
Reference in New Issue
Block a user