scanline_polygon_fill.h

Go to the documentation of this file.

#ifndef SCANLINE_POLYGON_FILL_H
#define SCANLINE_POLYGON_FILL_H

namespace cob_3d_mapping
{
  class ScanlinePolygon
  {
  public:
    ScanlinePolygon(const Eigen::Vector3f& n_, float d_)
    {
      float invN2 = 1.0f / n_(2);
      c(0) = d_ * invN2;
      c(1) = n_(0) * invN2;
      c(2) = n_(1) * invN2;
    }

    inline float intersection(float x, float y) const { return c(0) - c(1)*x - c(2)*y; }

    Eigen::Vector3f c;
  };

  /* data structure used by ScanlinePolygonFill
   * represents a line of a 2D polygon
   */
  template<typename T = int>
  class ScanlineEdge
  {
  public:
    ScanlineEdge(T id_, float x1, float y1, float x2, float y2)
      : id(id_)
      , x1_(x1)
      , y1_(y1)
      , x2_(x2)
      , y2_(y2)
    {
      if(y1>y2) { ymax = y1; ymin = y2; }
      else { ymax = y2; ymin = y1; }
      mInv = (x2-x1)/(y2-y1);
      t = mInv*y1/x1;
    }

    inline float intersection(float y) const { return mInv*(y-y1_)+x1_; }

    T id;
    float ymin;
    float ymax;
    float mInv;
    float t;
    float x1_;
    float y1_;
    float x2_;
    float y2_;
  };

  template<typename T>
  inline const bool operator< (const ScanlineEdge<T>& lhs,
                               const ScanlineEdge<T>& rhs)
  { return lhs.ymin < rhs.ymin; }

  template<typename T>
  inline const bool operator> (const ScanlineEdge<T>& lhs,
                               const ScanlineEdge<T>& rhs)
  { return  operator< (rhs, lhs); }

  template<typename T>
  inline const bool operator<= (const ScanlineEdge<T>& lhs,
                                const ScanlineEdge<T>& rhs)
  { return !operator> (lhs, rhs); }

  template<typename T>
  inline const bool operator>= (const ScanlineEdge<T>& lhs,
                                const ScanlineEdge<T>& rhs)
  { return !operator< (lhs, rhs); }


  /* Algorithm for rasterization of 2D images consisting of polygon lines
   * UPDATE: now modified to perform x,y,z clipping
   * Source: http://en.wikipedia.org/wiki/Flood_fill
   *         common practice in computer graphics
   * Output: image of which each pixel consists of a vector of ids
   */
  template<typename T = int>
  class ScanlinePolygonFill
  {
  public:
    ScanlinePolygonFill(int width, int height)
      : w(width)
      , h(height)
      , xmin(-1.0f)
      , xmax( 1.0f)
      , ymin(-1.0f)
      , ymax( 1.0f)
      , zmin(-1.0f)
      , zmax( 1.0f)
      , zthr( 0.2f)
    { }

    inline void addEdge(T id, float x1, float y1, float x2, float y2)
    {
      yque.push_back(ScanlineEdge<T>(id,x1,y1,x2,y2));
    }

    inline bool addPolygon(T id, const Eigen::Vector3f& normal, float d)
    {
      /*std::cout << "addPolygon: " << id <<": "<<normal(0)
        <<" x + "<<normal(1)<<" y + "<<normal(2)<<" z = "<<d<<std::endl;*/
      return polys.insert( std::make_pair(id, ScanlinePolygon(normal,d)) ).second;
    }

    inline int x2w(float x) const
    {
      return int((x - xmin) / (xmax - xmin) * float(w));
    }

    inline int y2h(float y) const
    {
      return int((y - ymin) / (ymax - ymin) * float(h));
    }

    // only draws lines of polygons
    void draw(std::vector<std::vector<T> >& out);

    // fills areas of polygons
    void fill(std::vector<std::vector<T> >& out);

  private:
    int w;
    int h;
    float xmin;
    float xmax;
    float ymin;
    float ymax;
    float zmin;
    float zmax;
    float zthr;
    std::list<ScanlineEdge<T> > yque;
    std::map<T,ScanlinePolygon> polys;
  };
}

#include "cob_3d_fov_segmentation/impl/scanline_polygon_fill.hpp"

#endif