initial commit

event_camera_simulator/esim/src/camera_simulator.cpp

@@ -0,0 +1,62 @@
+#include <esim/esim/camera_simulator.hpp>
+
+namespace event_camera_simulator {
+
+void ImageBuffer::addImage(Time t, const Image& img)
+{
+  if(!data_.empty())
+  {
+    // Check that the image timestamps are monotonically increasing
+    CHECK_GT(t, data_.back().stamp);
+  }
+
+  Duration exposure_time = data_.empty() ? 0 : t - data_.back().stamp;
+  VLOG(2) << "Adding image to buffer with stamp: " << t
+          << " and exposure time " << exposure_time;
+  data_.push_back(ImageData(img.clone(), t, exposure_time));
+
+  // Remove all the images with timestamp older than t - buffer_size_ns_
+  auto first_valid_element = std::lower_bound(data_.begin(), data_.end(), t - buffer_size_ns_,
+          [](ImageData lhs, Time rhs) -> bool { return lhs.stamp < rhs; });
+
+  data_.erase(data_.begin(), first_valid_element);
+  VLOG(3) << "first/last element in buffer: "
+          << data_.front().stamp
+          << " " << data_.back().stamp;
+  VLOG(3) << "number of images in the buffer: " << data_.size();
+
+  CHECK_LE(data_.back().stamp - data_.front().stamp, buffer_size_ns_);
+}
+
+
+bool CameraSimulator::imageCallback(const Image &img, Time time,
+                                         const ImagePtr& camera_image)
+{
+  CHECK(camera_image);
+  CHECK_EQ(camera_image->size(), img.size());
+
+  buffer_->addImage(time, img);
+
+  static const Time initial_time = time;
+  if(time - initial_time < exposure_time_)
+  {
+    LOG_FIRST_N(WARNING, 1) << "The images do not cover a time span long enough to simulate the exposure time accurately.";
+    return false;
+  }
+
+  // average all the images in the buffer to simulate motion blur
+  camera_image->setTo(0);
+  ze::real_t denom = 0.;
+  for(const ImageBuffer::ImageData& img : buffer_->getRawBuffer())
+  {
+    *camera_image += ze::nanosecToMillisecTrunc(img.exposure_time) * img.image;
+    denom += ze::nanosecToMillisecTrunc(img.exposure_time);
+  }
+  *camera_image /= denom;
+  cv::Mat disp;
+  camera_image->convertTo(disp, CV_8U, 255);
+
+  return true;
+}
+
+} // namespace event_camera_simulator

event_camera_simulator/esim/src/event_simulator.cpp

@@ -0,0 +1,117 @@
+#include <esim/esim/event_simulator.hpp>
+#include <ze/common/random.hpp>
+#include <glog/logging.h>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <ze/common/time_conversions.hpp>
+
+namespace event_camera_simulator {
+
+void EventSimulator::init(const Image &img, Time time)
+{
+  VLOG(1) << "Initialized event camera simulator with sensor size: " << img.size();
+  VLOG(1) << "and contrast thresholds: C+ = " << config_.Cp << " , C- = " << config_.Cm;
+  is_initialized_ = true;
+  last_img_ = img.clone();
+  ref_values_ = img.clone();
+  last_event_timestamp_ = TimestampImage::zeros(img.size());
+  current_time_ = time;
+  size_ = img.size();
+}
+
+Events EventSimulator::imageCallback(const Image& img, Time time)
+{
+  CHECK_GE(time, 0);
+  Image preprocessed_img = img.clone();
+  if(config_.use_log_image)
+  {
+    LOG_FIRST_N(INFO, 1) << "Converting the image to log image with eps = " << config_.log_eps << ".";
+    cv::log(config_.log_eps + img, preprocessed_img);
+  }
+
+  if(!is_initialized_)
+  {
+    init(preprocessed_img, time);
+    return {};
+  }
+
+  // For each pixel, check if new events need to be generated since the last image sample
+  static constexpr ImageFloatType tolerance = 1e-6;
+  Events events;
+  Duration delta_t_ns = time - current_time_;
+
+  CHECK_GT(delta_t_ns, 0u);
+  CHECK_EQ(img.size(), size_);
+
+  for (int y = 0; y < size_.height; ++y)
+  {
+    for (int x = 0; x < size_.width; ++x)
+    {
+      ImageFloatType itdt = preprocessed_img(y, x);
+      ImageFloatType it = last_img_(y, x);
+      ImageFloatType prev_cross = ref_values_(y, x);
+
+      if (std::fabs (it - itdt) > tolerance)
+      {
+        ImageFloatType pol = (itdt >= it) ? +1.0 : -1.0;
+        ImageFloatType C = (pol > 0) ? config_.Cp : config_.Cm;
+        ImageFloatType sigma_C = (pol > 0) ? config_.sigma_Cp : config_.sigma_Cm;
+        if(sigma_C > 0)
+        {
+          C += ze::sampleNormalDistribution<ImageFloatType>(false, 0, sigma_C);
+        }
+        ImageFloatType curr_cross = prev_cross;
+        bool all_crossings = false;
+
+        do
+        {
+          curr_cross += pol * C;
+
+          if ((pol > 0 && curr_cross > it && curr_cross <= itdt)
+              || (pol < 0 && curr_cross < it && curr_cross >= itdt))
+          {
+            Duration edt = (curr_cross - it) * delta_t_ns / (itdt - it);
+            Time t = current_time_ + edt;
+
+            // check that pixel (x,y) is not currently in a "refractory" state
+            // i.e. |t-that last_timestamp(x,y)| >= refractory_period
+            const Time last_stamp_at_xy = ze::secToNanosec(last_event_timestamp_(y,x));
+            CHECK_GE(t, last_stamp_at_xy);
+            const Duration dt = t - last_stamp_at_xy;
+            if(last_event_timestamp_(y,x) == 0 || dt >= config_.refractory_period_ns)
+            {
+              events.push_back(Event(x, y, t, pol > 0));
+              last_event_timestamp_(y,x) = ze::nanosecToSecTrunc(t);
+            }
+            else
+            {
+              VLOG(3) << "Dropping event because time since last event  ("
+                      << dt << " ns) < refractory period ("
+                      << config_.refractory_period_ns << " ns).";
+            }
+            ref_values_(y,x) = curr_cross;
+          }
+          else
+          {
+            all_crossings = true;
+          }
+        } while (!all_crossings);
+      } // end tolerance
+    } // end for each pixel
+  }
+
+  // update simvars for next loop
+  current_time_ = time;
+  last_img_ = preprocessed_img.clone(); // it is now the latest image
+
+  // Sort the events by increasing timestamps, since this is what
+  // most event processing algorithms expect
+  sort(events.begin(), events.end(),
+       [](const Event& a, const Event& b) -> bool
+  {
+    return a.t < b.t;
+  });
+
+  return events;
+}
+
+} // namespace event_camera_simulator

event_camera_simulator/esim/src/simulator.cpp

@@ -0,0 +1,137 @@
+#include <esim/esim/simulator.hpp>
+#include <ze/common/timer_collection.hpp>
+#include <esim/common/utils.hpp>
+
+namespace event_camera_simulator {
+
+DECLARE_TIMER(TimerEventSimulator, timers_event_simulator_,
+              simulate_events,
+              visualization
+              );
+
+Simulator::~Simulator()
+{
+  timers_event_simulator_.saveToFile("/tmp", "event_simulator.csv");
+}
+
+void Simulator::dataProviderCallback(const SimulatorData &sim_data)
+{
+  CHECK_EQ(event_simulators_.size(), num_cameras_);
+
+  if(sim_data.images_updated)
+  {
+    EventsVector events(num_cameras_);
+    Time time = sim_data.timestamp;
+    // simulate the events and camera images for every sensor in the rig
+    {
+      auto t = timers_event_simulator_[TimerEventSimulator::simulate_events].timeScope();
+      for(size_t i=0; i<num_cameras_; ++i)
+      {
+        events[i] = event_simulators_[i].imageCallback(*sim_data.images[i], time);
+
+        if(corrupted_camera_images_.size() < num_cameras_)
+        {
+          // allocate memory for the corrupted camera images and set them to 0
+          corrupted_camera_images_.emplace_back(std::make_shared<Image>(sim_data.images[i]->size()));
+          corrupted_camera_images_[i]->setTo(0.);
+        }
+
+        camera_simulators_[i].imageCallback(*sim_data.images[i], time, corrupted_camera_images_[i]);
+      }
+    }
+
+    // publish the simulation data + events
+    {
+      auto t = timers_event_simulator_[TimerEventSimulator::visualization].timeScope();
+      publishData(sim_data, events, corrupted_camera_images_);
+    }
+  }
+  else
+  {
+    {
+      // just forward the simulation data to the publisher
+      auto t = timers_event_simulator_[TimerEventSimulator::visualization].timeScope();
+      publishData(sim_data, {}, corrupted_camera_images_);
+    }
+  }
+}
+
+void Simulator::publishData(const SimulatorData& sim_data,
+                            const EventsVector& events,
+                            const ImagePtrVector& camera_images)
+{
+  if(publishers_.empty())
+  {
+    LOG_FIRST_N(WARNING, 1) << "No publisher available";
+    return;
+  }
+
+  Time time = sim_data.timestamp;
+  const Transformation& T_W_B = sim_data.groundtruth.T_W_B;
+  const TransformationVector& T_W_Cs = sim_data.groundtruth.T_W_Cs;
+  const ze::CameraRig::Ptr& camera_rig = sim_data.camera_rig;
+
+  // Publish the new data (events, images, depth maps, poses, point clouds, etc.)
+  if(!events.empty())
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->eventsCallback(events);
+  }
+  if(sim_data.poses_updated)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->poseCallback(T_W_B, T_W_Cs, time);
+  }
+  if(sim_data.twists_updated)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->twistCallback(sim_data.groundtruth.angular_velocities_,
+                              sim_data.groundtruth.linear_velocities_,
+                              time);
+  }
+  if(sim_data.imu_updated)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->imuCallback(sim_data.specific_force_corrupted, sim_data.angular_velocity_corrupted, time);
+  }
+
+  if(camera_rig)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->cameraInfoCallback(camera_rig, time);
+  }
+  if(sim_data.images_updated)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+    {
+      publisher->imageCallback(sim_data.images, time);
+
+      // the images should be timestamped at mid-exposure (unless it is not possible)
+      const Time mid_exposure_time = (time >= 0.5 * exposure_time_) ? time - 0.5 * exposure_time_ : time;
+      publisher->imageCorruptedCallback(camera_images, mid_exposure_time);
+    }
+  }
+  if(sim_data.depthmaps_updated)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->depthmapCallback(sim_data.depthmaps, time);
+  }
+  if(sim_data.optic_flows_updated)
+  {
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->opticFlowCallback(sim_data.optic_flows, time);
+  }
+  if(sim_data.depthmaps_updated && !events.empty())
+  {
+    PointCloudVector pointclouds(num_cameras_);
+    for(size_t i=0; i<num_cameras_; ++i)
+    {
+      CHECK(sim_data.depthmaps[i]);
+      pointclouds[i] = eventsToPointCloud(events[i], *sim_data.depthmaps[i], camera_rig->atShared(i));
+    }
+    for(const Publisher::Ptr& publisher : publishers_)
+      publisher->pointcloudCallback(pointclouds, time);
+  }
+}
+
+} // namespace event_camera_simulator