Simulate point clouds of packages loaded in trailers

generate_point_clouds.cpp

#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
#include <limits>
#include <vector>

typedef Eigen::Vector3f Vector3;
typedef Vector3::Scalar Length;

enum class Normal {INTERIOR, EXTERIOR};

class Face {
public:
  
  Face() = delete;
  
  Face(int axis, int surface)
  : axis_(axis)
  , surface_(surface >= 0 ? +1 : -1)
  {}
  
  int
  axis() const {return axis_;}
  
  int
  surface() const {return surface_;}
  
  bool
  does_intersect_ray_with_normal(Vector3 const& u, Normal normal) const {
    Length projection = surface_ * u[axis_];
    if (normal == Normal::EXTERIOR) {
      // test the direction opposite the normal of the surface
      projection = - projection;
    }
    return projection > 0;
  }
  
private:
  int axis_;
  int surface_;
};

class UnitInterval
{
public:
  
  UnitInterval()
  : min_(Vector3(1,1,-1)) // ignore z
  , max_(Vector3(-1,-1,+1))
  {}
  
  UnitInterval& operator+=(Vector3 const& u) {
    min_ = min_.cwiseMin(u);
    max_ = max_.cwiseMax(u);
    return *this;
  }
  
  UnitInterval&
  widen(float percentage) {
    Vector3 delta = max_ - min_;
    min_ -= percentage * delta;
    max_ += percentage * delta;
    return *this;
  }
  
  bool
  includes(Vector3 const& u) const {
    return (u.array() >= min_.array() && u.array() <= max_.array()).all();
  }
  
private:
  Vector3 min_;
  Vector3 max_;
};

class Box
{
  
public:
  typedef std::numeric_limits<Length> Limits;
  
  Box() = delete;
  
  Box(Vector3 const& top_left_front_corner, Length length)
  : Box(top_left_front_corner, Vector3(length, length, length))
  {}
  
  Box(Vector3 const& top_left_front_corner, Vector3 const& dimensions)
  : top_left_front_corner_(top_left_front_corner)
  , dimensions_(dimensions)
  {
    update_intervals();
  }
  
  Length
  frontal_area() const {
    return dimensions_.x() * dimensions_.y();
  }
  
  Length
  volume() const {
    return frontal_area() * dimensions_.z();
  }
  
  void
  move_forward() {
    top_left_front_corner_.z() -= dimensions_.z();
    update_intervals();
  }
  
  Length
  minimum_exterior_distance_along_ray(Vector3 const& u) const {
    float min_distance = Limits::infinity();
    
    if (interval_.includes(u)) {
      // bottom and back faces should never be visible
      for (auto const& face : {FRONT_FACE, RIGHT_FACE, LEFT_FACE, TOP_FACE}) {
        float d = distance_to_plane_along_ray(face, Normal::EXTERIOR, u);
        if (d < min_distance) {
          Vector3 intersection = u * d;
          if (is_point_within_bounds_along_axis(intersection, face.axis())) {
            min_distance = d;
          }
        }
      }
    }
    return min_distance;
  }
  
  Length
  minimum_interior_distance_along_ray(Vector3 const& u) const {
    float min_distance = Limits::infinity();
    
    for (auto const& face : {RIGHT_FACE, LEFT_FACE, TOP_FACE, BOTTOM_FACE, REAR_FACE}) {
      float d = distance_to_plane_along_ray(face, Normal::INTERIOR, u);
      if (d < min_distance) {
        min_distance = d;
      }
    }
    return min_distance;
  }
  
  static const Face LEFT_FACE;
  static const Face RIGHT_FACE;
  static const Face TOP_FACE;
  static const Face BOTTOM_FACE;
  static const Face FRONT_FACE;
  static const Face REAR_FACE;
  
  Length
  face_coordinate(Face const& face) const {
    return top_left_front_corner_[face.axis()] + (face.surface() > 0 ? dimensions_[face.axis()] : 0);
  }
  
private:
  
  Length
  distance_to_plane_along_ray(Face const& face, Normal normal, Vector3 const& u) const {
    return face.does_intersect_ray_with_normal(u, normal) ?
      face_coordinate(face) / u[face.axis()] : Limits::infinity();
  }
  
  bool
  is_point_within_bounds_along_axis(Vector3 const& point, int axis) const {
    for (int i = 0; i < 3; ++i) {
      if (i != axis &&
          (point[i] < face_coordinate(Face(i, -1)) ||
           point[i] > face_coordinate(Face(i, +1)))) {
            return false;
          }
    }
    return true;
  }
  
  void
  update_intervals() {
    UnitInterval interval;
    for (auto x : {Length(0), dimensions_.x()}) {
      for (auto y : {Length(0), dimensions_.y()}) {
        for (auto z : {Length(0), dimensions_.z()}) {
          Vector3 u = (top_left_front_corner_ + Vector3(x,y,z)).normalized();
          interval += u;
        }
      }
    }
    interval_ = interval.widen(0.1);
  }
  
  Vector3 top_left_front_corner_;
  Vector3 dimensions_;
  UnitInterval interval_;
};

const Face Box::LEFT_FACE   = Face(0, -1);
const Face Box::RIGHT_FACE  = Face(0, +1);
const Face Box::TOP_FACE    = Face(1, -1); // y axis-points down
const Face Box::BOTTOM_FACE = Face(1, +1);
const Face Box::FRONT_FACE  = Face(2, -1); // 'front' as in facing the camera, actually the -Z axis
const Face Box::REAR_FACE   = Face(2, +1);

class Trailer: public Box {

public:
  Trailer(Vector3 const& top_left_front_corner, Vector3 const& dimensions, Length package_dimension)
  : Box(top_left_front_corner, dimensions)
  , packages_()
  , package_count_(0)
  {
    Eigen::Vector3i num_packages = (dimensions / package_dimension).cast<int>();
    capacity_ = num_packages.x() * num_packages.y() * num_packages.z();
    
    // start the packages so their front face is the same plane as the
    // back of the trailer
    Length front_z = top_left_front_corner.z() + dimensions.z();
    // we want to put them in left to right, bottom to top
    // but remember, y axis points down
    for (int j = 0; j < num_packages.y(); ++j) {
      for (int i = 0; i < num_packages.x(); ++i) {
        Length left_x = top_left_front_corner.x() + i * package_dimension;
        Length top_y = top_left_front_corner.y() + dimensions.y() - (j+1) * package_dimension;
        packages_.push_back(new Box(Vector3(left_x, top_y, front_z), package_dimension));
      }
    }
  }
  
  int
  package_count() const {
    return package_count_;
  }
  
  bool
  full() const {
    return package_count_ >= capacity_;
  }
  
  void
  add_package() {
    if (full()) return;
    
    int i = package_count_ % packages_.size();
    packages_[i]->move_forward();
    package_count_ += 1;
  }
  
  Length
  minimum_distance_along_ray(Vector3 const& u) const {
    return std::min(minimum_distance_to_package_along_ray(u), minimum_interior_distance_along_ray(u));
  }
  
private:
  Length
  minimum_distance_to_package_along_ray(Vector3 const& u) const {
    Length min_distance = Limits::infinity();
    
    for (auto package : packages_) {
      Length d = package->minimum_exterior_distance_along_ray(u);
      if (d < min_distance) {
        min_distance = d;
      }
    }
    
    return min_distance;
  }
  
  std::vector<Box*> packages_;
  int package_count_;
  int capacity_;
};


class Camera
{
  typedef std::vector<Vector3> Points;
  typedef std::vector<Length> Depths;
  
public:
  Camera() = delete;
  
  Camera(int width, int height, float pixel_angle, float pitch = 0)
  : pitch_(pitch)
  , width_(width)
  , height_(height)
  , pixel_angle_(pixel_angle)
  , depths_(Depths(width*height))
  , rays_(Points(width*height))
  {
    initialize_rays();
  }
  
  void
  capture_point_cloud(Trailer const& trailer) {
    double cos_pitch = cos(pitch_);
    double sin_pitch = sin(pitch_);
    for (int i = 0; i < rays_.size(); ++i) {
      Vector3 const& camera_u = rays_[i];
      Vector3 global_u(camera_u.x(),
                       cos_pitch * camera_u.y() - sin_pitch * camera_u.z(),
                       sin_pitch * camera_u.y() + cos_pitch * camera_u.z());
      depths_[i] = trailer.minimum_distance_along_ray(global_u);
    }
  }
  
  void
  write_point_cloud(std::ostream &out) {
    write_head(out);
    for (int i = 0; i < depths_.size(); ++i) {
      write_point(rays_[i] * depths_[i], out);
    }
  }
  
private:
  
  void
  initialize_rays() {
    int center_i = width_ / 2;
    int center_j = height_ / 2;
    
    for (int j = 0; j < height_; ++j) {
      double j_angle = (j - center_j) * pixel_angle_;
      double tan_j = tan(j_angle);
      for (int i = 0; i < width_; ++i) {
        double i_angle = (i - center_i) * pixel_angle_;
        double tan_i = tan(i_angle);
        rays_[i + j * width_] = Vector3(tan_i, tan_j, 1).normalized();
      }
    }
  }
  
  void
  write_head(std::ostream& out) const {
    out
    << "VERSION 0.7\n"
    << "FIELDS x y z\n"
    << "SIZE 4 4 4\n"
    << "TYPE F F F\n"
    << "COUNT 1 1 1\n"
    << "WIDTH " << width_ << "\n"
    << "HEIGHT " << height_ << "\n"
    << "VIEWPOINT 0 0 0 1 0 0 0\n"
    << "POINTS " << width_ * height_ << "\n"
    << "DATA binary\n";
  }
  
  void
  write_point(Vector3 point, std::ostream& out) const {
    out.write((char const*)point.data(), 3 * sizeof(Length));
  }
  
  float pitch_;
  int width_;
  int height_;
  double pixel_angle_;
  Depths depths_;
  Points rays_;
};

std::string
pcd_filename_for_package(int p) {
  std::string package_number_string = std::to_string(p);
  package_number_string.insert(0,std::max(0,4-(int)package_number_string.size()), '0');
  return "package_" + package_number_string + ".pcd";
}

int main(int argc, const char * argv[])
{
  Vector3 trailer_dimensions(2,2,10);
  float package_dimension = 0.5;
  Vector3 trailer_top_left_front(- trailer_dimensions.x() / 2.0, - 0.3, 0.3);
  Trailer trailer(trailer_top_left_front, trailer_dimensions, package_dimension);
  int width = 320;
  int height = 240;
  float pixel_angle = .0038;
  float pitch = -0.25;
  Camera camera(width, height, pixel_angle, pitch);
  for (; !trailer.full(); trailer.add_package()) {
    std::ofstream output(pcd_filename_for_package(trailer.package_count()), std::ios::trunc);
    camera.capture_point_cloud(trailer);
    camera.write_point_cloud(output);
    output.close();
  }
  return (0);
}