text: #include "dvs_integrator/integrator.h"
  #include <cv_bridge/cv_bridge.h>
  #include <math.h>
  
  namespace dvs_integrator {
  
  Integrator::Integrator(ros::NodeHandle & nh, ros::NodeHandle nh_private)
    : nh_(nh),
      t_last_(0)
  {
    // Get parameters of the method
    nh_private.param<double>("cut_off", alpha_cutoff_, 5.);
  
    // Set up subscribers and publishers
    // FILL IN...
    event_sub_ = nh_.subscribe("events", 0, &Integrator::eventsCallback, this);
    // Set the queue size to infinity to avoid dropping messages.
  
    image_transport::ImageTransport it_(nh_);
    // FILL IN...
    time_map_pub_ = it_.advertise("time_map", 1);
    image_pub_ = it_.advertise("image_out", 1);
  
    // Dynamic reconfigure
    dynamic_reconfigure_callback_ = boost::bind(&Integrator::reconfigureCallback, this, _1, _2);
    server_.reset(new dynamic_reconfigure::Server<dvs_integrator::dvs_integratorConfig>(nh_private));
    server_->setCallback(dynamic_reconfigure_callback_);
  
    // Contrast thresholds
    // Later we may use varying contrast sensitivities
    c_pos_ = 0.1;
    c_neg_ = 0.1;
  }
  
  
  Integrator::~Integrator()
  {
    // Close the publishers
    image_pub_.shutdown();
  }
  
  
  /**
   * Process input event messages
   */
  void Integrator::eventsCallback(const dvs_msgs::EventArray::ConstPtr& msg)
  {
    // Need to update the state (time_map and brightness image) even if there are no subscribers on the output image
  
    const double t_first_ = msg->events[0].ts.toSec();
  
    if (!(state_time_map_.rows == msg->height && state_time_map_.cols == msg->width) || t_last_ > t_first_)
    {
      // Allocate memory for time map and for image out
      // Initialize the time map with the time of the first event
      // and initialize the brightness image to zero.
      // FILL IN...
      state_time_map_ = cv::Mat(msg->height, msg->width, CV_64FC1, cv::Scalar(t_first_));
      state_image_    = cv::Mat::zeros(msg->height, msg->width, CV_64FC1);
    }
  
    // Process events in the message msg, one by one (in a loop)
    // FILL IN...
    for (const dvs_msgs::Event& ev : msg->events)
    {
      double beta = exp(-alpha_cutoff_ * (ev.ts.toSec() - state_time_map_.at<double>(ev.y, ev.x)));
      state_image_.at<double>(ev.y, ev.x) = beta * state_image_.at<double>(ev.y, ev.x);
  
      if (ev.polarity == true)
      {
        state_image_.at<double>(ev.y, ev.x) += c_pos_;
      }
      else
      {
        state_image_.at<double>(ev.y, ev.x) -= c_neg_;
      }
  
      state_time_map_.at<double>(ev.y, ev.x) = ev.ts.toSec();
  
    }
  
    // Exit if there are no subscribers
    if (image_pub_.getNumSubscribers() + time_map_pub_.getNumSubscribers() > 0)
    {
      publishState();
    }
  }
  
  
  /**
   * Publish the time map and the brightness image at latest time (Output)
   */
  void Integrator::publishState()
  {
    // Publish the current state (time map and image)
  
    // Initialize, including header
    cv_bridge::CvImage cv_image, cv_image_time;
    cv_image.header.stamp = ros::Time::now();
    cv_image.encoding = "mono8";
    cv_image_time.header.stamp = ros::Time::now();
    cv_image_time.encoding = "mono8";
  
    // Need to display all pixels at the same reference time (e.g., the time of
    // the last event). So do not forget to "decay" the brightness to the ref. time.
    // FILL IN...
    double max_ts, _;
    cv::minMaxLoc(state_time_map_, &_, &max_ts);
  
    cv::Mat image_normalized;
    cv::exp(-alpha_cutoff_ * (max_ts - state_time_map_), image_normalized); // = beta
    image_normalized = image_normalized.mul(state_image_);
  
    double rmin, rmax;
    double percentage_pixels_to_discard = 0.1;
    minMaxLocRobust(image_normalized, rmin, rmax, percentage_pixels_to_discard);
    
    double slope;
    if (rmax == rmin)
    {
      slope = 1;
    }
    else
    {
      slope = 255. / (rmax - rmin);
    }
  
    image_normalized = slope * (image_normalized - rmin);
    image_normalized.convertTo(cv_image.image, CV_8UC1);
  
    // Convert to appropriate range, [0,255]
    // Feel free to use minMaxLocRobust
    // FILL IN...
    minMaxLocRobust(state_time_map_, rmin, rmax, percentage_pixels_to_discard);
    t_last_ = rmax;
  
    if (rmax == rmin)
    {
      slope = 1;
    }
    else
    {
      slope = 255. / (rmax - rmin);
    }
  
    cv::Mat time_map_normalized;
    time_map_normalized = slope * (state_time_map_ - rmin);
    time_map_normalized.convertTo(cv_image_time.image, CV_8UC1);
  
    // Publish the time map and the brightness image
    time_map_pub_.publish(cv_image_time.toImageMsg());
    image_pub_.publish(cv_image.toImageMsg());
  }
  
  
  /**
   * Interface with the parameters that can be changed online via dynamic reconfigure
   */
  void Integrator::reconfigureCallback(dvs_integrator::dvs_integratorConfig &config, uint32_t level)
  {
    alpha_cutoff_ = config.Cutoff_frequency;
  }
  
  
  /**
   * Compute the robust min and max pixel values of an image using percentiles,
   * e.g., 2th and 98th percentiles instead of min and max, respectively.
   *
   * Sort the pixel values of an image and discard a percentage of them,
   * from the top and the bottom, as potential outliers to compute
   * a robust version of the min and max values.
   */
  void Integrator::minMaxLocRobust(const cv::Mat& image, double& rmin, double& rmax,
                                   const double& percentage_pixels_to_discard)
  {
    cv::Mat image_as_row = image.reshape(0,1);
    cv::Mat image_as_row_sorted;
    cv::sort(image_as_row, image_as_row_sorted, CV_SORT_EVERY_ROW + CV_SORT_ASCENDING);
    image_as_row_sorted.convertTo(image_as_row_sorted, CV_64FC1);
    const int single_row_idx_min = (0.5*percentage_pixels_to_discard/100.)*image.total();
    const int single_row_idx_max = (1 - 0.5*percentage_pixels_to_discard/100.)*image.total();
    rmin = image_as_row_sorted.at<double>(single_row_idx_min);
    rmax = image_as_row_sorted.at<double>(single_row_idx_max);
  }
  
  } // namespace
  