EssentialPoint2Line.hh

#ifndef __EssentialPoint2Line_hh__
#define __EssentialPoint2Line_hh__

#include <MathAlgo/UnscentedTransform.hh>

namespace BIAS {

  /** computes line and associated covariance matrix given by a point and an 
      essential matrix using unscented transform 
      @author woelk 07/2007 */
  class EssentialPoint2Line : public BIAS::UnscentedTransform
  {
  public:
    int Compute(const BIAS::HomgPoint2D& p2, const BIAS::HomgPoint2DCov &cov2, 
                const BIAS::EParametrization &E, BIAS::Vector3<double> &line, 
                BIAS::Matrix3x3<double> &cov_line)
    {
      BIAS::Vector<double> src(10), dst, Ev;
      BIAS::Matrix<double> src_cov(10, 10, BIAS::MatrixZero), dst_cov, E_cov;

      Ev = E.GetEQ7();
      E_cov = E.GetCov();
      for (int c=0; c<7; c++){
        src[c] = Ev[c];
        for (int r=0; r<7; r++){
          src_cov[r][c] = E_cov[r][c];
        }      
      }
    
      for (int c=0; c<3; c++){
        src[c+7] = p2[c];
        for (int r=0; r<3; r++){
          src_cov[r+7][c+7] = cov2[r][c];
        }      
      }

      // TODO: enforce symmetry in lines above, this is slow:
      src_cov.MakeSymmetric();

      int res =  Transform(src, src_cov, dst, dst_cov);
      line = dst;
      cov_line = dst_cov;
      return res;
    }

  protected:

    virtual int Transform_(const BIAS::Vector<double>& src, 
                           BIAS::Vector<double>& dst) const
    {
      BIAS::Vector<double> Ev(7);
      BIAS::Vector3<double> p;
      for (int i=0; i<7; i++)
        Ev[i] = src[i];
      for (int i=0; i<3; i++)
        p[i] = src[i+7];
    
      BIAS::EParametrization E;
      E.SetEQ(Ev);
    
      dst = (BIAS::Vector<double>)(E.GetEssentialMatrix() * p);
      return 0;
    }

  }; // class

} // namespace 


#endif // __EssentialPoint2Line_hh__