Covariance3Dto2DHomg.hh

#ifndef _Covariance3Dto2DHomg_hh_
#define _Covariance3Dto2DHomg_hh_

#include <MathAlgo/UnscentedTransform.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Geometry/PMatrixBase.hh>

namespace BIAS {
  class PoseParametrization;
  class HomgPoint2D;
  class HomgPoint2DCov;
  class HomgPoint3D;
  class HomgPoint3DCov;
  template <class T> class Matrix;
  template <class T> class Vector;

#define D_C3DT2D_COV 0x00000100

  /** @class Covariance3Dto2DHomg
      @brief Projection of 3D points with uncertainties covariances into the 
      2d image plane taking the camera pose uncertainty into account.

      Given a point in 3D with uncertainty represented/approximated by a 
      multivariate normal distribution (i.e. 3x3 covariance matrix).
      Given further a camera pose (camer center and orientation).

      The algorithm computes the projection of the 3D point into the image
      taking the camera pose uncertainty into account. The probabilty 
      distribution function in the image is represented by a 2D normal 
      distribution (i.e. a 2D covariance matrix).

      The resulting 2D covariance matrix is approximated using the unscented
      transform. See UnscentedTransform for details.

      To use it instantiate this class with the appropriate parametrization
      of the camera orientation. Currently Euler angles, 
      axis (norm 1) and angle, axis times angle and quaternion are supported.
      
      See Example/ExampleCovariance3Dto2DHomg for exemplary usage.

      TODO: implement for point normalization instead of homogenization

      @author woelk 02/2006 */
  class BIASGeometry_EXPORT Covariance3Dto2DHomg
    : public BIAS::UnscentedTransform
  {
  public:
    Covariance3Dto2DHomg(const BIAS::KMatrix& K, 
                     const enum BIAS::PMatrixBase::E_ParametrizationType par);

    ~Covariance3Dto2DHomg();

    /** compute */
    int Project(const BIAS::PoseParametrization& Pose,
                const BIAS::HomgPoint3D& X,
                const BIAS::HomgPoint3DCov& Cov_X,
                BIAS::HomgPoint2D& x,
                BIAS::HomgPoint2DCov& Cov_x) const;

    inline void SetK(const BIAS::KMatrix& K)
    { K_ = K; }

    inline void UseHomogenizedPoints(bool h) {
      useHomogenizedPoints_ = h;
    }
    
  protected:
    /// normalize or homogenize ???
    bool useHomogenizedPoints_;
    BIAS::KMatrix K_;
    enum BIAS::PMatrixBase::E_ParametrizationType Parametrization_;

    /** src is a vector consisting of [ 3d point, camera center, orientation ]
        The orientation must be represented using one case from 
        E_Parametrization
        dst is the projection into the image */
    virtual int Transform_(const BIAS::Vector<double>& src,
                           BIAS::Vector<double>& dst) const;
  

  };
} // namespace

#endif // _Covariance3Dto2DHomg_hh_