Add subscriber to read frames. Plot red-blue events

README.md

@@ -2,9 +2,10 @@
 
 Bare minimum event processing in ROS C++.
 
-At this point, the visualizer only reads event messages and displays them as a (published) image.
+At this point, the visualizer can read events and frames from a DAVIS (event messages and image messages from two topics) and displays them as a (published) image.
 
 - Added a parameter `display_method` in the launch file `display_monocular.launch` to select between grayscale and red-blue event image.
+- Added capability to read grayscale frame and plot red-blue events on top.
 
 ### Dependencies
 

include/dvs_displayer/displayer.h

@@ -3,6 +3,7 @@
 #include <ros/ros.h>
 #include <image_transport/image_transport.h>
 #include <dvs_msgs/EventArray.h>
+#include <opencv2/core/core.hpp>
 
 namespace dvs_displayer
 {
@@ -17,12 +18,16 @@ class Displayer {
 
   // Callback functions
   void eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg);
+  void imageCallback(const sensor_msgs::Image::ConstPtr& msg);
 
   // Subscribers
   ros::Subscriber event_sub_;
+  image_transport::Subscriber image_sub_;
 
   // Publishers
   image_transport::Publisher image_pub_;
+  cv::Mat last_image_;
+  bool used_last_image_;
 
   enum DisplayMethod
   {

launch/display_monocular.launch

@@ -3,6 +3,7 @@
   <node name="dvs_displayer_one" pkg="dvs_displayer" type="dvs_displayer" output="screen">
     <param name="display_method" value="red-blue"/>
     <remap from="events" to="/dvs/events" />
+    <remap from="image" to="/dvs/image_raw" />
     <remap from="event_image" to="event_image" />
   </node>
 

src/displayer.cpp

@@ -1,6 +1,5 @@
 #include "dvs_displayer/displayer.h"
 #include <cv_bridge/cv_bridge.h>
-#include <opencv2/core/core.hpp>
 
 namespace dvs_displayer {
 
@@ -16,11 +15,13 @@ Displayer::Displayer(ros::NodeHandle & nh, ros::NodeHandle nh_private) : nh_(nh)
   {
       display_method_ = RED_BLUE;
   }
+  used_last_image_ = false;
 
   // Set up subscribers and publishers
   event_sub_ = nh_.subscribe("events", 1, &Displayer::eventsCallback, this);
 
   image_transport::ImageTransport it_(nh_);
+  image_sub_ = it_.subscribe("image", 1, &Displayer::imageCallback, this);
   image_pub_ = it_.advertise("event_image", 1);
 }
 
@@ -31,6 +32,37 @@ Displayer::~Displayer()
 }
 
 
+void Displayer::imageCallback(const sensor_msgs::Image::ConstPtr& msg)
+{
+  cv_bridge::CvImagePtr cv_ptr;
+
+  try
+  {
+    cv_ptr = cv_bridge::toCvCopy(msg);
+  }
+  catch (cv_bridge::Exception& e)
+  {
+    ROS_ERROR("cv_bridge exception: %s", e.what());
+    return;
+  }
+
+  // Convert to BGR image
+  if (msg->encoding == "mono8")
+  {
+    cv::cvtColor(cv_ptr->image, last_image_, CV_GRAY2BGR);
+  }
+
+  if (!used_last_image_)
+  {
+    cv_bridge::CvImage cv_image;
+    last_image_.copyTo(cv_image.image);
+    cv_image.encoding = "bgr8";
+    image_pub_.publish(cv_image.toImageMsg());
+  }
+  used_last_image_ = false;
+}
+
+
 void Displayer::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg)
 {
 
@@ -49,9 +81,19 @@ void Displayer::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg)
 
   if (display_method_ == GRAYSCALE)
   {
-    // Create image
     cv_image.encoding = "mono8";
-    cv_image.image = cv::Mat(msg->height, msg->width, CV_8U, cv::Scalar(128));
+
+    if (last_image_.rows == msg->height && last_image_.cols == msg->width)
+    {
+      // If DAVIS image is available, use it as canvas
+      cv::cvtColor(last_image_, cv_image.image, CV_BGR2GRAY);
+      used_last_image_ = true;
+    }
+    else
+    {
+      // Create image
+      cv_image.image = cv::Mat(msg->height, msg->width, CV_8U, cv::Scalar(128));
+    }
 
     // Per-pixel event histograms
     cv::Mat on_events  = cv::Mat::zeros(msg->height, msg->width, CV_8U);
@@ -77,9 +119,19 @@ void Displayer::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg)
   }
   else if (display_method_ == RED_BLUE)
   {
-    // Create image
     cv_image.encoding = "bgr8";
-    cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3, cv::Scalar(255,255,255));
+
+    if (last_image_.rows == msg->height && last_image_.cols == msg->width)
+    {
+      // If DAVIS image is available, use it as canvas
+      last_image_.copyTo(cv_image.image);
+      used_last_image_ = true;
+    }
+    else
+    {
+      // Create image
+      cv_image.image = cv::Mat(msg->height, msg->width, CV_8UC3, cv::Scalar(255,255,255));
+    }
 
     // Just red or blue over white background
     for(const dvs_msgs::Event& ev : msg->events)