initial commit

event_camera_simulator/imp/imp_planar_renderer/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(imp_planar_renderer)
+
+find_package(catkin_simple REQUIRED)
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+catkin_simple()
+
+set(HEADERS
+    include/esim/imp_planar_renderer/planar_renderer.hpp
+)
+
+set(SOURCES
+    src/planar_renderer.cpp
+)
+
+cs_add_library(${PROJECT_NAME} ${SOURCES} ${HEADERS})
+
+cs_install()
+cs_export()

event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/panorama_renderer.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <esim/rendering/renderer_base.hpp>
+
+namespace event_camera_simulator {
+
+//! A rendering engine for planar scenes
+class PanoramaRenderer : public Renderer
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  PanoramaRenderer(const Image& texture,
+                   const Transformation::Rotation& R_W_P);
+
+  ~PanoramaRenderer();
+
+  //! Render image and depth map for a given camera pose
+  virtual void render(const Transformation& T_W_C, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const override;
+
+  virtual void setCamera(const ze::Camera::Ptr& camera) override;
+
+protected:
+
+  void precomputePixelToBearingLookupTable();
+  void projectToPanorama(const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint) const;
+
+  // Texture mapped on the plane
+  Image texture_;
+  Transformation::Rotation R_W_P_;
+
+  // Intrinsic parameters of the panorama camera
+  FloatType fx_, fy_;
+  ze::Keypoint principal_point_;
+
+  // Precomputed lookup table from keypoint -> bearing vector
+  ze::Bearings bearings_C_;
+
+  // Preallocated warping matrices
+  mutable cv::Mat_<float> map_x_, map_y_;
+};
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/planar_renderer.hpp

@@ -0,0 +1,59 @@
+#pragma once
+
+#include <esim/rendering/renderer_base.hpp>
+
+namespace event_camera_simulator {
+
+//! A rendering engine for planar scenes
+class PlanarRenderer : public Renderer
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  PlanarRenderer(const Image& texture,
+                 const Camera::Ptr& cam_src,
+                 const Transformation &T_W_P,
+                 FloatType z_min,
+                 FloatType z_max,
+                 bool extend_border);
+
+  ~PlanarRenderer();
+
+  //! Render image and depth map for a given camera pose
+  virtual void render(const Transformation& T_W_C, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const;
+  void render(const Transformation& T_W_C, const std::vector<Transformation>& T_W_OBJ, const ImagePtr &out_image, const DepthmapPtr &out_depthmap) const override
+  {
+      render(T_W_C, out_image, out_depthmap);
+  }
+
+  //! Returns true if the rendering engine can compute optic flow, false otherwise
+  virtual bool canComputeOpticFlow() const override { return false; }
+
+  virtual void setCamera(const ze::Camera::Ptr& camera) override;
+
+protected:
+
+  void precomputePixelToBearingLookupTable();
+
+  // Texture mapped on the plane
+  Image texture_;
+  Camera::Ptr cam_src_;
+  CalibrationMatrix K_src_, K_src_inv_;
+
+  // Plane parameters
+  Transformation T_W_P_;
+
+  // Clipping parameters
+  FloatType z_min_;
+  FloatType z_max_;
+
+  bool extend_border_;
+
+  // Precomputed lookup table from keypoint -> bearing vector
+  ze::Bearings bearings_C_;
+
+  // Preallocated warping matrices
+  mutable cv::Mat_<float> map_x_, map_y_;
+};
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_base.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <ze/common/macros.hpp>
+#include <esim/common/types.hpp>
+
+namespace event_camera_simulator {
+
+//! Represents a rendering engine that generates images (and other outputs,
+//! such as depth maps, or optical flow maps) given a scene and a camera position.
+class Renderer
+{
+public:
+  ZE_POINTER_TYPEDEFS(Renderer);
+
+  Renderer() {}
+
+  //! Render an image at a given pose.
+  virtual void render(const Transformation& T_W_C,
+                      const ImagePtr& out_image,
+                      const DepthmapPtr& out_depthmap) const = 0;
+
+  //! Render an image + depth map + optic flow map at a given pose,
+  //! given the camera linear and angular velocity
+  virtual void render(const Transformation& T_W_C,
+                      const LinearVelocity& v_WC,
+                      const AngularVelocity& w_WC,
+                      const ImagePtr& out_image,
+                      const DepthmapPtr& out_depthmap,
+                      const OpticFlowPtr& optic_flow_map) const {}
+
+  
+  //! Sets the camera
+  virtual void setCamera(const ze::Camera::Ptr& camera) = 0;
+
+  //! Get the camera rig
+  const ze::Camera::Ptr& getCamera() const { return camera_; }
+
+protected:
+  ze::Camera::Ptr camera_;
+};
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/include/esim/imp_planar_renderer/renderer_factory.hpp

@@ -0,0 +1,11 @@
+#pragma once
+
+#include <gflags/gflags.h>
+#include <esim/rendering/renderer_base.hpp>
+
+namespace event_camera_simulator {
+
+bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat *img);
+Renderer::Ptr loadRendererFromGflags();
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>imp_planar_renderer</name>
+  <version>0.0.0</version>
+  <description>Rendering engine that simulates camera motion in a planar scene, parameterized by the plane equation and texture.</description>
+
+  <maintainer email="rebecq@ifi.uzh.ch">Henri Rebecq</maintainer>
+
+  <license>GPLv3</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>catkin_simple</buildtool_depend>
+
+  <depend>esim_common</depend>
+  <depend>esim_rendering</depend>
+  <depend>ze_common</depend>
+  <depend>ze_cameras</depend>
+  <depend>gflags_catkin</depend>
+  <depend>glog_catkin</depend>
+
+</package>

event_camera_simulator/imp/imp_planar_renderer/src/panorama_renderer.cpp

@@ -0,0 +1,113 @@
+#include <esim/common/utils.hpp>
+#include <esim/rendering/panorama_renderer.hpp>
+
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <glog/logging.h>
+#include <opencv2/core/eigen.hpp>
+
+namespace event_camera_simulator {
+
+PanoramaRenderer::PanoramaRenderer(const Image& texture,
+                                   const Transformation::Rotation &R_W_P)
+  : texture_(texture),
+    R_W_P_(R_W_P)
+{
+  principal_point_ = ze::Keypoint(0.5 * texture_.cols,
+                                  0.5 * texture_.rows);
+  fx_ = texture_.cols / (2.0 * CV_PI);
+  fy_ = texture_.rows / CV_PI;
+  LOG(INFO) << "Initialized panoramic camera with size: "
+            << texture_.cols << "x" << texture_.rows
+            << ", principal point: " << principal_point_.transpose()
+            << ", fx = " << fx_ << "px , fy = " << fy_ << " px";
+}
+
+PanoramaRenderer::~PanoramaRenderer()
+{
+}
+
+void PanoramaRenderer::setCamera(const ze::Camera::Ptr& camera)
+{
+  CHECK(camera);
+  camera_ = camera;
+
+  precomputePixelToBearingLookupTable();
+
+  // Preallocate memory for the warping maps
+  cv::Size sensor_size(camera->width(), camera->height());
+  map_x_ = cv::Mat_<float>::zeros(sensor_size);
+  map_y_ = cv::Mat_<float>::zeros(sensor_size);
+}
+
+
+void PanoramaRenderer::precomputePixelToBearingLookupTable()
+{
+  // points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
+  ze::Keypoints points_C(2, camera_->width() * camera_->height());
+  for(int y=0; y<camera_->height(); ++y)
+  {
+    for(int x=0; x<camera_->width(); ++x)
+    {
+      points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
+    }
+  }
+  bearings_C_ = camera_->backProjectVectorized(points_C);
+  bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
+}
+
+
+void PanoramaRenderer::projectToPanorama(const Eigen::Ref<const ze::Bearing>& f, ze::Keypoint* keypoint) const
+{
+  CHECK(keypoint);
+  static constexpr FloatType kEpsilon = 1e-10;
+
+  const FloatType X = f[0];
+  const FloatType Y = f[1];
+  const FloatType Z = f[2];
+
+  const FloatType rho2 = X*X+Y*Y+Z*Z;
+  const FloatType rho = std::sqrt(rho2);
+
+  CHECK_GE(rho, kEpsilon);
+
+  const FloatType phi = std::atan2(X, Z);
+  const FloatType theta = std::asin(-Y/rho);
+
+  *keypoint = principal_point_ + ze::Keypoint(phi * fx_, -theta * fy_);
+}
+
+
+void PanoramaRenderer::render(const Transformation& T_W_C, const ImagePtr& out_image, const DepthmapPtr& out_depthmap) const
+{
+  CHECK_EQ(out_image->rows, camera_->height());
+  CHECK_EQ(out_image->cols, camera_->width());
+
+  const Transformation::RotationMatrix R_P_C = R_W_P_.inverse().getRotationMatrix() * T_W_C.getRotationMatrix();
+
+  ze::Bearings bearings_P = R_P_C * bearings_C_;
+  ze::Keypoint keypoint;
+  for(int y=0; y<camera_->height(); ++y)
+  {
+    for(int x=0; x<camera_->width(); ++x)
+    {
+      const ze::Bearing& f = bearings_P.col(x + camera_->width() * y);
+
+      // project bearing vector to panorama image
+      projectToPanorama(f, &keypoint);
+
+      map_x_(y,x) = static_cast<float>(keypoint[0]);
+      map_y_(y,x) = static_cast<float>(keypoint[1]);
+    }
+  }
+
+  cv::remap(texture_, *out_image, map_x_, map_y_, CV_INTER_LINEAR, cv::BORDER_REFLECT_101);
+
+  if(out_depthmap)
+  {
+    static constexpr ImageFloatType infinity = 1e10;
+    out_depthmap->setTo(infinity);
+  }
+}
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/src/planar_renderer.cpp

@@ -0,0 +1,146 @@
+#include <esim/common/utils.hpp>
+#include <esim/imp_planar_renderer/planar_renderer.hpp>
+
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <glog/logging.h>
+#include <opencv2/core/eigen.hpp>
+#include <ze/common/timer_collection.hpp>
+
+DECLARE_TIMER(TimerPlanarRenderer, timers_planar_renderer,
+              compute_remap_maps,
+              inverse_homography,
+              remap,
+              compute_depthmap
+              );
+
+namespace event_camera_simulator {
+
+PlanarRenderer::PlanarRenderer(const Image& texture,
+                               const Camera::Ptr &cam_src,
+                               const Transformation& T_W_P,
+                               FloatType z_min,
+                               FloatType z_max,
+                               bool extend_border)
+  : texture_(texture),
+    cam_src_(cam_src),
+    T_W_P_(T_W_P),
+    z_min_(z_min),
+    z_max_(z_max),
+    extend_border_(extend_border)
+{
+  K_src_ = calibrationMatrixFromCamera(cam_src_);
+  K_src_inv_ = K_src_.inverse();
+  VLOG(1) << "K_src = " << K_src_;
+
+  LOG(INFO) << "T_W_P = " << T_W_P_;
+}
+
+PlanarRenderer::~PlanarRenderer()
+{
+  timers_planar_renderer.saveToFile("/tmp", "planar_renderer.csv");
+}
+
+void PlanarRenderer::setCamera(const ze::Camera::Ptr& camera)
+{
+  CHECK(camera);
+  camera_ = camera;
+
+  precomputePixelToBearingLookupTable();
+
+  // Preallocate memory for the warping maps
+  cv::Size sensor_size(camera->width(), camera->height());
+  map_x_ = cv::Mat_<float>::zeros(sensor_size);
+  map_y_ = cv::Mat_<float>::zeros(sensor_size);
+}
+
+
+void PlanarRenderer::precomputePixelToBearingLookupTable()
+{
+  // points_C is a matrix containing the coordinates of each pixel coordinate in the image plane
+  ze::Keypoints points_C(2, camera_->width() * camera_->height());
+  for(int y=0; y<camera_->height(); ++y)
+  {
+    for(int x=0; x<camera_->width(); ++x)
+    {
+      points_C.col(x + camera_->width() * y) = ze::Keypoint(x,y);
+    }
+  }
+  bearings_C_ = camera_->backProjectVectorized(points_C);
+  bearings_C_.array().rowwise() /= bearings_C_.row(2).array();
+}
+
+void PlanarRenderer::render(const Transformation& T_W_C, const ImagePtr& out_image, const DepthmapPtr& out_depthmap) const
+{
+  CHECK_EQ(out_image->rows, camera_->height());
+  CHECK_EQ(out_image->cols, camera_->width());
+
+  const Transformation T_C_W = T_W_C.inverse();
+  const Transformation T_C_P = T_C_W * T_W_P_;
+  const Transformation T_P_C = T_C_P.inverse();
+
+  ze::Matrix33 tmp;
+  tmp.col(0) = T_C_P.getRotationMatrix().col(0);
+  tmp.col(1) = T_C_P.getRotationMatrix().col(1);
+  tmp.col(2) = T_C_P.getPosition();
+
+  VLOG_EVERY_N(3, 100) << "T_C_P = " << T_C_P;
+  VLOG_EVERY_N(3, 100) << "tmp = " << tmp;
+  HomographyMatrix H_C_P = tmp;
+
+  HomographyMatrix H_P_C;
+  {
+    auto t = timers_planar_renderer[TimerPlanarRenderer::inverse_homography].timeScope();
+    H_P_C = H_C_P.inverse();
+  }
+
+  {
+    auto t = timers_planar_renderer[TimerPlanarRenderer::compute_remap_maps].timeScope();
+    ze::Bearings bearings_P = H_P_C * bearings_C_;
+    for(int y=0; y<camera_->height(); ++y)
+    {
+      for(int x=0; x<camera_->width(); ++x)
+      {
+        const ze::Bearing& f = bearings_P.col(x + camera_->width() * y);
+        map_x_(y,x) = static_cast<float>(K_src_(0,0) * f[0]/f[2] + K_src_(0,2));
+        map_y_(y,x) = static_cast<float>(K_src_(1,1) * f[1]/f[2] + K_src_(1,2));
+      }
+    }
+  }
+
+  int border = (extend_border_) ? cv::BORDER_REFLECT : cv::BORDER_CONSTANT;
+  {
+    auto t = timers_planar_renderer[TimerPlanarRenderer::remap].timeScope();
+    cv::remap(texture_, *out_image, map_x_, map_y_, CV_INTER_LINEAR, border, 0.8);
+  }
+
+  // Compute depth map in dst
+  {
+    auto t = timers_planar_renderer[TimerPlanarRenderer::compute_depthmap].timeScope();
+    Vector3 X;
+    for(int y=0; y<camera_->height(); ++y)
+    {
+      for(int x=0; x<camera_->width(); ++x)
+      {
+        X = bearings_C_.col(x + camera_->width() * y);
+        // @TODO: derive this formula for the depth to explain it
+        const FloatType numer = -T_P_C.getPosition()[2];
+        const FloatType denom = T_P_C.getRotationMatrix().row(2) * X;
+        const FloatType z = numer / denom;
+
+        if(out_depthmap)
+        {
+          (*out_depthmap)(y,x) = (ImageFloatType) z;
+        }
+
+        // clip depth to a reasonable depth range
+        if(z < z_min_ || z > z_max_)
+        {
+          (*out_image)(y,x) = 0;
+        }
+      }
+    }
+  }
+}
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/src/renderer_base.cpp

@@ -0,0 +1,5 @@
+#include <esim/rendering/renderer_base.hpp>
+
+namespace event_camera_simulator {
+
+} // namespace event_camera_simulator

event_camera_simulator/imp/imp_planar_renderer/src/renderer_factory.cpp

@@ -0,0 +1,164 @@
+#include <esim/common/utils.hpp>
+#include <esim/rendering/renderer_factory.hpp>
+#include <esim/rendering/planar_renderer.hpp>
+#include <esim/rendering/panorama_renderer.hpp>
+#include <ze/cameras/camera_models.hpp>
+#include <ze/cameras/camera_impl.hpp>
+#include <ze/common/logging.hpp>
+#include <opencv2/imgcodecs/imgcodecs.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+DEFINE_int32(renderer_type, 0, " 0: Planar renderer");
+
+DEFINE_string(renderer_texture, "",
+              "Path to image which will be used to texture the plane");
+
+DEFINE_double(renderer_hfov_cam_source_deg, 130.0,
+              "Horizontal FoV of the source camera (that captured the image on the plane)");
+
+DEFINE_double(renderer_preprocess_median_blur, 0,
+              "Kernel size of the preprocessing median blur.");
+
+DEFINE_double(renderer_preprocess_gaussian_blur, 0,
+              "Amount of preprocessing Gaussian blur.");
+
+DEFINE_double(renderer_plane_x, 0.0,
+              "x position of the center of the plane, in world coordinates");
+
+DEFINE_double(renderer_plane_y, 0.0,
+              "y position of the center of the plane, in world coordinates");
+
+DEFINE_double(renderer_plane_z, -1.0,
+              "z position of the center of the plane, in world coordinates");
+
+DEFINE_double(renderer_plane_qw, 0.0,
+              "w component of the quaternion q_W_P (orientation of the plane with respect to the world)");
+
+DEFINE_double(renderer_plane_qx, 1.0,
+              "x component of the quaternion q_W_P (orientation of the plane with respect to the world)");
+
+DEFINE_double(renderer_plane_qy, 0.0,
+              "y component of the quaternion q_W_P (orientation of the plane with respect to the world)");
+
+DEFINE_double(renderer_plane_qz, 0.0,
+              "z component of the quaternion q_W_P (orientation of the plane with respect to the world)");
+
+DEFINE_double(renderer_z_min, 0.01,
+              "Minimum clipping distance.");
+
+DEFINE_double(renderer_z_max, 10.0,
+              "Maximum clipping distance.");
+
+DEFINE_bool(renderer_extend_border, false,
+              "Whether to extend the borders of the plane to infinity or not.");
+
+DEFINE_double(renderer_panorama_qw, 0.0,
+              "w component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
+
+DEFINE_double(renderer_panorama_qx, 1.0,
+              "x component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
+
+DEFINE_double(renderer_panorama_qy, 0.0,
+              "y component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
+
+DEFINE_double(renderer_panorama_qz, 0.0,
+              "z component of the quaternion q_W_P (orientation of the panorama with respect to the world)");
+
+namespace event_camera_simulator {
+
+bool loadPreprocessedImage(const std::string& path_to_img, cv::Mat* img)
+{
+  CHECK(img);
+  VLOG(1) << "Loading texture file from file: " << FLAGS_renderer_texture << ".";
+  *img = cv::imread(path_to_img, 0);
+  if(!img->data)
+  {
+    LOG(FATAL) << "Could not open image at: " << FLAGS_renderer_texture << ".";
+    return false;
+  }
+
+  if(FLAGS_renderer_preprocess_median_blur > 1)
+  {
+    VLOG(1) << "Pre-filtering the texture with median filter of size: "
+            << FLAGS_renderer_preprocess_median_blur << ".";
+    cv::medianBlur(*img, *img, FLAGS_renderer_preprocess_median_blur);
+  }
+
+  if(FLAGS_renderer_preprocess_gaussian_blur > 0)
+  {
+    VLOG(1) << "Pre-filtering the texture with gaussian filter of size: "
+            << FLAGS_renderer_preprocess_gaussian_blur << ".";
+    cv::GaussianBlur(*img, *img, cv::Size(15,15), FLAGS_renderer_preprocess_gaussian_blur);
+  }
+
+  img->convertTo(*img, cv::DataType<ImageFloatType>::type, 1.0/255.0);
+
+  return true;
+}
+
+Renderer::Ptr loadRendererFromGflags()
+{
+  Renderer::Ptr renderer;
+
+  switch (FLAGS_renderer_type)
+  {
+    case 0: // Planar renderer
+    {
+      cv::Mat img_src;
+      if(!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
+      {
+        return nullptr;
+      }
+
+      double f_src = hfovToFocalLength(FLAGS_renderer_hfov_cam_source_deg, img_src.cols);
+      Camera::Ptr cam_src = std::make_shared<ze::PinholeCamera>(
+                      img_src.cols, img_src.rows, ze::CameraType::Pinhole,
+                      (Vector4() << f_src, f_src, 0.5 * img_src.cols, 0.5 * img_src.rows).finished(),
+                      ze::VectorX());
+
+      Transformation T_W_P;
+      T_W_P.getPosition() = ze::Position(FLAGS_renderer_plane_x,
+                                         FLAGS_renderer_plane_y,
+                                         FLAGS_renderer_plane_z);
+
+      T_W_P.getRotation() = ze::Quaternion(FLAGS_renderer_plane_qw,
+                                            FLAGS_renderer_plane_qx,
+                                            FLAGS_renderer_plane_qy,
+                                            FLAGS_renderer_plane_qz);
+
+      renderer.reset(new PlanarRenderer(img_src, cam_src,
+                                        T_W_P,
+                                        FLAGS_renderer_z_min,
+                                        FLAGS_renderer_z_max,
+                                        FLAGS_renderer_extend_border));
+
+      break;
+    }
+    case 1: // Panorama renderer
+    {
+       cv::Mat img_src;
+       if(!loadPreprocessedImage(FLAGS_renderer_texture, &img_src))
+       {
+         return nullptr;
+       }
+
+        Transformation::Rotation R_W_P;
+        R_W_P = ze::Quaternion(FLAGS_renderer_panorama_qw,
+                               FLAGS_renderer_panorama_qx,
+                               FLAGS_renderer_panorama_qy,
+                               FLAGS_renderer_panorama_qz);
+
+        renderer.reset(new PanoramaRenderer(img_src, R_W_P));
+      break;
+    }
+    default:
+    {
+      LOG(FATAL) << "Renderer type is not known.";
+      break;
+    }
+  }
+
+  return renderer;
+}
+
+} // namespace event_camera_simulator