Cov3Dto2DPoseCertain.hh

#ifndef __Cov3Dto2DPoseCertain_hh__
#define __Cov3Dto2DPoseCertain_hh__

#include <MathAlgo/UnscentedTransform.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Geometry/HomgPoint3D.hh>
#include <Base/Geometry/PMatrixBase.hh>
#include <Base/Math/Vector.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Vector3.hh>
#include <Base/Math/Matrix3x3.hh>
#include <Base/Math/Vector2.hh>
#include <Base/Math/Matrix2x2.hh>

namespace BIAS {

  /** @class Cov3Dto2DPoseCertain
      @brief project a 3D covariance matrix into the image plane using the 
      unscented transform assuming that the pose is certain
      @author woelk 06/2006 */
  class Cov3Dto2DPoseCertain : public BIAS::UnscentedTransform
  {
  public:
    inline Cov3Dto2DPoseCertain(const BIAS::PMatrixBase& P) 
      : UnscentedTransform(), P_(P) {};
  
    inline void Transform(const BIAS::Vector3<double>& src_mean, 
                          const BIAS::Matrix3x3<double>& src_cov,
                          BIAS::Vector2<double>& dst_mean, 
                          BIAS::Matrix2x2<double>& dst_cov) const
    {
      BIAS::Vector<double> src_mean2(src_mean), dst_mean2;
      BIAS::Matrix<double> src_cov2(src_cov), dst_cov2;
      UnscentedTransform::Transform(src_mean2, src_cov2, dst_mean2, dst_cov2);
      dst_mean = dst_mean2;
      dst_cov = dst_cov2;
    }

  protected:
    BIAS::PMatrixBase P_;
    int Transform_(const BIAS::Vector<double>& src, 
                   BIAS::Vector<double>& dst) const
    {
      BIAS::HomgPoint3D X;
      BIAS::HomgPoint2D x;
  
      X.Set(src[0], src[1], src[2], 1.0);
      x = P_ * X;
      x.Homogenize();

      dst.newsize(2);
      dst[0] = x[0];
      dst[1] = x[1];
      return 0;
    }
  }; 


} // namespace

#endif // __Cov3Dto2DPoseCertain_hh__