sensor_model.h

Go to the documentation of this file.

#ifndef __SENSOR_MODEL_H__
#define __SENSOR_MODEL_H__

#include "cob_3d_mapping_common/polygon.h"
#include <list>
#include <algorithm>

namespace cob_3d_mapping
{
  class PrimeSense
  {
  public:
    typedef Eigen::Map<const Eigen::Matrix<float,4,4,Eigen::RowMajor> > MatMap;

    PrimeSense() {}
    ~PrimeSense() {}

    /*
     * \brief Check if two points are neighbors.
     *
     * \param[in] query The depth of the query point.
     * \param[in] neighbor The depth of the potential neighbor.
     * \param[in] tolerance The distance tolerance factor.
     *
     * \return True, if the points are neighbors.
     */
    inline static bool areNeighbors(float query,
                             float neighbor,
                             float tolerance = 2.0f)
    {
      float dist_th = tolerance * 0.003f * query * query;
      //if (query < 1.2) dist_th += 0.01f;
      return (fabs(query - neighbor) < dist_th);
    }

    /*
     * \brief Check if two points are neighbors.
     *
     * \param[in] query The query point.
     * \param[in] neighbor The potential neighbor.
     * \param[in] tolerance The distance tolerance factor.
     *
     * \return True, if the points are neighbors.
     */
    inline static bool areNeighbors(const Eigen::Vector3f& query,
                             const Eigen::Vector3f&  neighbor,
                             float tolerance = 4.0f)
    {
      // += max( 0, 4-10*min(|x|,|y|) )
      tolerance += max( 0.0f, 1.0f * (4.0f - min(fabs(query(0)),fabs(query(1))) * 10.0f) );
      float dist_th =  tolerance * 0.003f * query(2) * query(2);
      return (fabs(query(2) - neighbor(2)) < dist_th);
    }

    inline static Eigen::Matrix4f tf_unit_cube() { return MatMap(mat); }

    // frustum parameters:
    static const float fov_h; 
    static const float fov_v; 
    static const float f; 
    static const float n; 
    static const float l; 
    static const float r; 
    static const float t; 
    static const float b; 
    static const float w;
    static const float h;
    static const float aspect;
    static const float mat[]; // transformation from view frustum to unit cube
  };

  const float PrimeSense::fov_h = 49.0f / 180.0f * M_PI;
  const float PrimeSense::fov_v = 43.0f / 180.0f * M_PI;
  const float PrimeSense::f = 5.0f;
  const float PrimeSense::n = 0.5f;
  const float PrimeSense::r = PrimeSense::n * tan(PrimeSense::fov_h * 0.5f);
  const float PrimeSense::l = - PrimeSense::r;
  const float PrimeSense::t = PrimeSense::n * tan(PrimeSense::fov_v * 0.5f);
  const float PrimeSense::b = - PrimeSense::t;
  const float PrimeSense::w = 640.0f;
  const float PrimeSense::h = 480.0f;
  const float PrimeSense::aspect = PrimeSense::w / PrimeSense::h;

  const float PrimeSense::mat[] =
  {
    1.0f / ( PrimeSense::aspect * tan(0.5f * PrimeSense::fov_h) ),
    0,
    0,
    0,

    0,
    1.0f / ( tan(0.5f * PrimeSense::fov_h) ),
    0,
    0,

    0,
    0,
    (PrimeSense::n + PrimeSense::f) / (PrimeSense::n - PrimeSense::f),
    -2.0f * PrimeSense::f * PrimeSense::n / (PrimeSense::n - PrimeSense::f),

    0,
    0,
    1,
    0
  };

}

#endif