initial commit

event_camera_simulator/esim_visualization/src/adaptive_sampling_benchmark_publisher.cpp

@@ -0,0 +1,135 @@
+#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 <gflags/gflags.h>
+#include <glog/logging.h>
+
+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.");
+
+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_);
+
+  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, "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);
+
+    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;
+  }
+
+  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();
+}
+
+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);
+
+  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";
+
+    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;
+  }
+
+  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::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;
+  }
+}
+
+} // namespace event_camera_simulator

event_camera_simulator/esim_visualization/src/py/optic_flow_converter.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+"""
+    Node that listens to esim_msgs/OpticFlow messages, and publishes it
+    as a color-coded image, and as a vector field.
+"""
+
+import rospy
+import numpy as np
+import cv2
+from sensor_msgs.msg import Image
+from esim_msgs.msg import OpticFlow
+from cv_bridge import CvBridge
+
+class FlowConverterNode:
+    _p0 = None
+
+    def __init__(self):
+        self._bridge = CvBridge()
+
+        rospy.Subscriber("flow", OpticFlow, self._OpticFlowCallback)
+        self.pub_color = rospy.Publisher("flow_color", Image, queue_size=0)
+        self.pub_arrows = rospy.Publisher("flow_arrows", Image, queue_size=0)
+
+        self.arrows_step = rospy.get_param('~arrows_step', 12)
+        self.arrows_scale = rospy.get_param('~arrows_scale', 0.1)
+        self.arrows_upsample_factor = rospy.get_param('~arrows_upsample_factor', 2)
+        self.publish_rate = rospy.get_param('~publish_rate', 20)
+
+        rospy.loginfo('Started flow converter node')
+        rospy.loginfo('Step size between arrows: {}'.format(self.arrows_step))
+        rospy.loginfo('Scale factor: {:.2f}'.format(self.arrows_scale))
+        rospy.loginfo('Upsample factor: x{}'.format(self.arrows_upsample_factor))
+        rospy.loginfo('Publish rate: {} Hz'.format(self.publish_rate))
+
+        self.arrows_color = (0,0,255) # red
+        self.first_msg_received = False
+
+    def reset(self):
+        self.first_msg_received = False
+        self.last_msg_stamp = None
+
+    def _OpticFlowCallback(self, msg):
+        if not self.first_msg_received:
+            rospy.logdebug('Received first message at stamp: {}'.format(msg.header.stamp.to_sec()))
+            self.last_msg_stamp = msg.header.stamp
+            self.first_msg_received = True
+            self.convertAndPublishFlow(msg)
+
+        if msg.header.stamp.to_sec() < self.last_msg_stamp.to_sec():
+            rospy.loginfo('Optic flow converter reset because new stamp is older than the latest stamp')
+            self.reset()
+            return
+
+        time_since_last_published_msg = (msg.header.stamp - self.last_msg_stamp).to_sec()
+        rospy.logdebug('Time since last published message: {}'.format(time_since_last_published_msg))
+        if time_since_last_published_msg >= 1./float(self.publish_rate):
+            self.last_msg_stamp = msg.header.stamp
+            self.convertAndPublishFlow(msg)
+
+    def convertAndPublishFlow(self, msg):
+        height, width = msg.height, msg.width
+        flow_x = np.array(msg.flow_x).reshape((height, width))
+        flow_y = np.array(msg.flow_y).reshape((height, width))
+        self.publishColorCodedFlow(flow_x, flow_y, msg.header.stamp)
+        self.publishArrowFlow(flow_x, flow_y, msg.header.stamp)
+
+    def publishColorCodedFlow(self, flow_x, flow_y, stamp):
+        assert(flow_x.shape == flow_y.shape)
+        height, width = flow_x.shape
+        magnitude, angle = cv2.cartToPolar(flow_x, flow_y)
+        magnitude = cv2.normalize(src=magnitude, dst=magnitude, alpha=0, beta=255, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_8U)
+
+        hsv = np.zeros((height,width,3), dtype=np.uint8)
+        self.hsv = hsv.copy()
+        hsv[...,1] = 255
+        hsv[...,0] = 0.5 * angle * 180 / np.pi
+        hsv[...,2] = cv2.normalize(magnitude,None,0,255,cv2.NORM_MINMAX)
+        bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR)
+
+        img_msg = self._bridge.cv2_to_imgmsg(bgr, 'bgr8')
+        img_msg.header.stamp = stamp
+        self.pub_color.publish(img_msg)
+
+    def publishArrowFlow(self, flow_x, flow_y, stamp):
+        assert(flow_x.shape == flow_y.shape)
+        height, width = flow_x.shape
+
+        step = self.arrows_step
+        scale = self.arrows_scale
+        ss = self.arrows_upsample_factor
+        arrow_field = np.zeros((ss * height, ss * width,3), dtype=np.uint8)
+
+        for x in np.arange(0, width, step):
+            for y in np.arange(0, height, step):
+                vx = flow_x[y,x]
+                vy = flow_y[y,x]
+                cv2.arrowedLine(arrow_field, (ss * x, ss * y),
+                                (int(ss * (x + scale * vx)), int(ss * (y + scale * vy))), color=self.arrows_color, thickness=1)
+
+        img_msg = self._bridge.cv2_to_imgmsg(arrow_field, 'bgr8')
+        img_msg.header.stamp = stamp
+        self.pub_arrows.publish(img_msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node('flow_converter_node')
+    node = FlowConverterNode()
+    rospy.spin()

event_camera_simulator/esim_visualization/src/ros_publisher.cpp

@@ -0,0 +1,400 @@
+#include <esim/visualization/ros_publisher.hpp>
+#include <esim/common/utils.hpp>
+#include <ze/common/time_conversions.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>
+
+DEFINE_double(ros_publisher_camera_info_rate, 10,
+"Camera info (maximum) publish rate, in Hz");
+
+DEFINE_double(ros_publisher_frame_rate, 25,
+"(Maximum) frame rate, in Hz");
+
+DEFINE_double(ros_publisher_depth_rate, 25,
+"(Maximum) depthmap publish rate, in Hz");
+
+DEFINE_double(ros_publisher_pointcloud_rate, 25,
+"(Maximum) point cloud publish rate, in Hz");
+
+DEFINE_double(ros_publisher_optic_flow_rate, 25,
+"(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_);
+
+  // 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_));
+
+  // 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_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)));
+
+    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)));
+
+    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)));
+  }
+
+  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;
+    }
+
+    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;
+    }
+
+    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;
+}
+
+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;
+}
+
+void RosPublisher::imageCorruptedCallback(const ImagePtrVector& corrupted_images, Time t)
+{
+  CHECK_EQ(image_corrupted_pub_.size(), num_cameras_);
+
+  for(size_t i=0; i<num_cameras_; ++i)
+  {
+    CHECK(image_corrupted_pub_[i]);
+    if(image_corrupted_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_corrupted_image_time_ > 0 && t - last_published_corrupted_image_time_ < min_time_interval_between_published_images_)
+    {
+      return;
+    }
+
+    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;
+}
+
+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;
+}
+
+
+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;
+    }
+
+    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(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;
+}
+
+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);
+  }
+}
+
+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

event_camera_simulator/esim_visualization/src/ros_utils.cpp

@@ -0,0 +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>
+
+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);
+  }
+
+  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]);
+    }
+  }
+}
+
+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;
+  // TODO: Add R and P
+}
+
+
+} // namespace event_camera_simulator

event_camera_simulator/esim_visualization/src/rosbag_writer.cpp

@@ -0,0 +1,308 @@
+#include <esim/visualization/rosbag_writer.hpp>
+#include <esim/common/utils.hpp>
+#include <ze/common/time_conversions.hpp>
+#include <esim/visualization/ros_utils.hpp>
+#include <minkindr_conversions/kindr_msg.h>
+#include <minkindr_conversions/kindr_tf.h>
+#include <tf/tfMessage.h>
+
+#include <gflags/gflags.h>
+#include <glog/logging.h>
+
+DECLARE_double(ros_publisher_camera_info_rate);
+DECLARE_double(ros_publisher_frame_rate);
+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.");
+
+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_);
+
+  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;
+
+  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;
+    }
+
+    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;
+}
+
+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;
+    }
+
+    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;
+    }
+
+    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::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;
+}
+
+
+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(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);
+    }
+  }
+
+  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(events[i].empty())
+    {
+      continue;
+    }
+
+    dvs_msgs::EventArrayPtr msg;
+    msg.reset(new dvs_msgs::EventArray);
+    eventsToMsg(events[i], sensor_sizes_[i].width, sensor_sizes_[i].height, msg);
+
+    bag_.write(getTopicName(topic_name_prefix_, i, "events"),
+               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;
+
+  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);
+
+    // 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);
+  }
+
+  transform_stamped_msg.child_frame_id = "body";
+  tf::transformKindrToMsg(T_W_B, &transform_stamped_msg.transform);
+  tf_msg.transforms.push_back(transform_stamped_msg);
+
+  bag_.write("/tf", toRosTime(t), tf_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_);
+
+  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

event_camera_simulator/esim_visualization/src/synthetic_optic_flow_publisher.cpp

@@ -0,0 +1,107 @@
+#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 <gflags/gflags.h>
+#include <glog/logging.h>
+
+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_);
+}
+
+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);
+}
+
+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);
+
+  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;
+
+     // 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
+     }
+  }
+
+  // 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);
+
+  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();
+}
+
+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);
+  }
+}
+
+} // namespace event_camera_simulator