projection.hpp

Go to the documentation of this file.

#ifndef COB_3D_PROJECTION_HPP
#define COB_3D_PROJECTION_HPP

#include "cob_3d_fov_segmentation/scanline_polygon_fill.h"

namespace cob_3d_mapping
{
  template<typename SensorT>
  void PerspectiveProjection<SensorT>::compute(
    const Mat4& tf_camera, int w, int h,
    const std::vector<cob_3d_mapping::Polygon::Ptr>& polygons,
    std::vector<std::vector<int> >& projection)
  {
    // create transformation matrix from sensor position and camera parameters
    Mat4 proj = SensorT::tf_unit_cube() * tf_camera;

    // transform polygons to clipping space and reject outliers
    // ( space: x,y,z E [-1,1] )
    ScanlinePolygonFill<> spf(w,h);
    Vec4 projected;
    float xprev, yprev, xcurr, ycurr;
    for(int i=0; i<polygons.size(); ++i)
    {
      int id = polygons[i]->id_;
      std::vector<std::vector<Vec3> > contours = polygons[i]->getContours3D();

      Vec4 p1 = proj * makeVector4f( contours[0][1] );
      Vec4 p2 = proj * makeVector4f( contours[0][2] );
      Vec4 p3 = proj * makeVector4f( contours[0][0] );

      p1 /= p1(3);
      p2 /= p2(3);
      p3 /= p3(3);
      Vec3 a = p2.head<3>() - p1.head<3>();
      Vec3 b = p3.head<3>() - p1.head<3>();
      Vec3 normal = (a.cross(b)).normalized();
      float d = p1.head<3>().dot(normal);
      spf.addPolygon(id, normal, d);

      for(int c=0; c<contours.size(); ++c)
      {
        std::vector<Vec3>* ptr_c = &contours[c];
        projected = proj * makeVector4f( (*ptr_c)[ ptr_c->size()-1 ] );
        float inv_w = 1.0f / projected(3);
        xprev = projected(0) * inv_w;
        yprev = projected(1) * inv_w;

        for(int p=0; p<ptr_c->size(); ++p)
        {
          projected = proj * makeVector4f((*ptr_c)[p]);
          inv_w = 1.0f / projected(3);
          xcurr = projected(0) * inv_w;
          ycurr = projected(1) * inv_w;
          spf.addEdge(id, xprev, yprev, xcurr, ycurr);
          xprev = xcurr;
          yprev = ycurr;
        }
      }
    }
    // rasterize remaining polygons using scanline rendering algorithm
    spf.fill(projection);
  }
}

#endif