Covariance3Dto2D.hh

#ifndef _Covariance3Dto2D_hh_
#define _Covariance3Dto2D_hh_

#include <MathAlgo/UnscentedTransform.hh>
#include <Base/Geometry/KMatrix.hh>
#include <Base/Geometry/PMatrixBase.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Vector2.hh>
#include <Base/Math/Matrix2x2.hh>

namespace BIAS {

#define D_C3DT2D_COV 0x00000100

  /** @class Covariance3Dto2D
      @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/ExampleCovariance3Dto2D for exemplary usage.

      TODO: implement for point normalization instead of homogenization

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

    ~Covariance3Dto2D();

    /** project covariance onto w=1 plane in camera coordinate system
        @return 0: ok, -1 if ray behind/parallel to that plane */
    int Project(const BIAS::Vector<double>& Pose,
                const BIAS::Matrix<double>& Cov_Pose,
                const BIAS::Vector3<double>& X,
                const BIAS::Matrix3x3<double>& Cov_X,
                BIAS::Vector2<double>& x,
                BIAS::Matrix2x2<double>& Cov_x) const;

    inline void SetK(const BIAS::KMatrix& K)
    { K_ = K; }
    
  protected:
    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 // _Covariance3Dto2D_hh_