LocalAffineFrame.hh

#ifndef _LOCALAFFINEFRAME_HH_
#define _LOCALAFFINEFRAME_HH_

#include <bias_config.h>

#ifdef BIAS_HAVE_XML2
  #include <Base/Common/XMLBase.hh>
#endif

#include <Base/Math/Vector.hh>
#include <Base/Math/Vector2.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Image/ImageBase.hh>
#include <Base/Math/Matrix.hh>
#include <Base/Math/Matrix2x2.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Geometry/HMatrix.hh>

// make very small nonzero default uncertainty for gaussian methods
#define LAF_DEFAULT_UNCERTAINTY 1e-10

namespace BIAS {

  /** @class LocalAffineFrame
      @brief affine transformation of 2D image plane which relates image 
      coordinate system and local affine feature coordinate system

     
      @author koeser 11/2007
  */
      

  class BIASGeometry_EXPORT LocalAffineFrame
#ifdef BIAS_HAVE_XML2
    : public BIAS::XMLBase
#endif
  { 
  public:
    
    inline LocalAffineFrame(): t_(0,0), A_(BIAS::MatrixIdentity), 
      Cov_(6,6,BIAS::MatrixZero) {
      for (unsigned int i=0; i<6; i++) Cov_[i][i] = LAF_DEFAULT_UNCERTAINTY;
    };
        
    inline LocalAffineFrame(const LocalAffineFrame &FP) { 
      (*this) = FP; 
    };

    const LocalAffineFrame& operator=(const LocalAffineFrame &FP) {
      t_ = FP.t_;
      A_ = FP.A_;
      Cov_ = FP.Cov_;
      return (*this);
    }

    /** @brief tranform a point in the image coordinate system into the feature
        coordinate system */
    BIAS::HomgPoint2D GlobalToLocal(const BIAS::HomgPoint2D& x) const {
      BIAS::Vector2<double> xeu(x[0]/x[2]-t_[0], x[1]/x[2]-t_[1]);
      return BIAS::HomgPoint2D(A_.Invert() * xeu);
    }

    /** @brief tranform a point in the feature coordinate system into the 
        iamge coordinate system */
    BIAS::HomgPoint2D LocalToGlobal(const BIAS::HomgPoint2D& x) const {
      BIAS::Vector2<double> xeu(x[0]/x[2], x[1]/x[2]);
      return BIAS::HomgPoint2D(A_ * xeu + t_);
    }

    /** @brief find closest scaled rotation matrix to linear A (under 
        Frobenius norm) and return */
    BIAS::LocalAffineFrame GetLocalSimilarityFrame() const;

    /** @brief return a 6-vector (a11,a12,a21,a22,tx,ty) */
    BIAS::Vector<double> GetAsVector() const {
      BIAS::Vector<double> a(6);
      const double *pd = A_.GetData();
      a[0] = *pd++;
      a[1] = *pd++;
      a[2] = *pd++;
      a[3] = *pd++;
      a[4] = t_[0];
      a[5] = t_[1];
      return a;
    }

    /** @brief set from a 6-vector (a11,a12,a21,a22,tx,ty), default cov */
    void SetFromVector(const BIAS::Vector<double>& affineparams) {
      SetFromVector(affineparams, 
                    BIAS::Matrix<double>(6,6,BIAS::MatrixZero));
      SetDefaultCov();
    }

    /** @brief set from 6-vector (a11,a12,a21,a22,tx,ty) with cov */
    void SetFromVector(const BIAS::Vector<double>& affineparams, 
                       const BIAS::Matrix<double>& cov) {
      Cov_ = cov;
      double *pd = A_.GetData();
      *pd++ = affineparams[0];
      *pd++ = affineparams[1];
      *pd++ = affineparams[2];
      *pd++ = affineparams[3];
      t_[0] = affineparams[4];
      t_[1] = affineparams[5];
    }

    /** @brief return 6x6 covariance */
    const BIAS::Matrix<double>& GetCov() const {
      return Cov_;
    }

    /** @brief returns matrix which transforms a point in feature coordinates
        into a point in image coordinates (LocalToGlobal) */
    BIAS::Matrix<double> GetAsMatrix() const {
      BIAS::Matrix<double> M(3,3,BIAS::MatrixZero);
      M[0][0] = A_[0][0];
      M[0][1] = A_[0][1];
      M[1][0] = A_[1][0]; 
      M[1][1] = A_[1][1];
      M[0][2] = t_[0];
      M[1][2] = t_[1];
      M[2][2] = 1;
      return M;
    }

    /** @brief returns matrix which transforms a point in image coordinates
        into a point in feature coordinates (GlobalToLocal) */
    BIAS::Matrix<double> GetAsInverseMatrix() const {
      BIAS::Matrix<double> M(3, 3, BIAS::MatrixZero);
      BIAS::Matrix2x2<double> A = A_.Invert();
      M[0][0] = A[0][0];
      M[0][1] = A[0][1];
      M[1][0] = A[1][0]; 
      M[1][1] = A[1][1];
      BIAS::Vector2<double> offset = A*t_;
      M[0][2] = -offset[0];
      M[1][2] = -offset[1];
      M[2][2] = 1;
      return M;
    }

    /** @brief construct from homography-style affine matrix (last row 0,0,1)*/
    void SetFromMatrix(const BIAS::Matrix<double>& affineTransform) {
      BIASASSERT(affineTransform.num_rows()==3);
      BIASASSERT(affineTransform.num_cols()==3);
      BIASASSERT(fabs(affineTransform[2][0])<1e-8);
      BIASASSERT(fabs(affineTransform[2][1])<1e-8);
      BIASASSERT(BIAS::Equal(affineTransform[2][2],1.0));
      A_[0][0] = affineTransform[0][0];
      A_[0][1] = affineTransform[0][1];
      A_[1][0] = affineTransform[1][0];
      A_[1][1] = affineTransform[1][1];
      t_[0]    = affineTransform[0][2];
      t_[1]    = affineTransform[1][2];
      SetDefaultCov();
    }

    /** @brief sets default nonzero cov */
    inline void SetDefaultCov() {
      Cov_.SetZero();
      for (unsigned int i=0; i<6; i++) Cov_[i][i] = LAF_DEFAULT_UNCERTAINTY;
    }

    /** @brief return affine matrix */
    const BIAS::Matrix2x2<double>& GetA() const { return A_; }

    /** @brief return offset */
    const BIAS::Vector2<double>& GetT() const { return t_; }

    /** @brief return affine matrix */
    void SetA(const BIAS::Matrix2x2<double>& A, 
              const BIAS::Matrix<double>& cov = 
              BIAS::Matrix<double>(4,4,BIAS::MatrixZero)) {
      A_ = A; 

      // erase correlations between offset and old affine params and copy cov
      for (unsigned int x=0; x<4; x++) {
        Cov_[x][4] = 0;
        Cov_[x][5] = 0;
        Cov_[4][x] = 0;
        Cov_[5][x] = 0;
        for (unsigned int y=x; y<4; y++) {
          Cov_[y][x] = Cov_[x][y] =  0.5*(cov[y][x]+cov[x][y]);
        }
      }
    }

    /** @brief return offset */
    void SetT(const BIAS::Vector2<double>& T, 
              const BIAS::Matrix<double>& cov = 
              BIAS::Matrix<double>(2,2,BIAS::MatrixZero)) { 
      t_ = T; 
      // erase correlations between old offset and affine params
      for (unsigned int x=0; x<4; x++) {
        Cov_[x][4] = 0;
        Cov_[x][5] = 0;
        Cov_[4][x] = 0;
        Cov_[5][x] = 0;
      }
      Cov_[4][4] = cov[0][0];
      Cov_[4][5] = Cov_[5][4] = 0.5*(cov[1][0] + cov[0][1]);
      Cov_[5][5] = cov[1][1];
    }

    /** @brief get global transform to transfer an image point of 1 into the
        image system of 2, e.g. given two affine features, get affine transform
        between images for KLT tracker 
        @brief setDefaultCov set digonal small cov or use error propagation
        from uncertainties of base transformations */
    LocalAffineFrame GetRelativeTransform(const LocalAffineFrame& F2,
                                          bool setDefaultCov = false) const;

    /** @brief use linear propagation to transform the local affine frame e.g.
        into another image 

        Definition of homography direction:
        (*this) has "position" t_. The result will have position H*t_

    */
    LocalAffineFrame GetHomographyTransformed(const BIAS::HMatrix& H) const;


    /** @brief draw a square at the feature's ori position 
        @author koeser */
    void Draw(BIAS::Image<unsigned char>& targetImage, 
              const double& positionCovScale = 1.0,
              const unsigned int linethickness=1) const;
   
    /** @brief returns fraction of square area [-1;1][-1;1], which is in the 
        image (slow numeric sampling) */
    double FractionInImage(unsigned int width, unsigned int height,
                           unsigned int x0=0, unsigned int y0=0) const {
      int count = 0;
      for (int x=-5; x<=5; x++) {
        for (int y=-5; y<=5; y++) {
          HomgPoint2D l(x,y,5);
          l.Homogenize();
          l = LocalToGlobal(l);
          l.Homogenize();
          if ((l[0]>x0) && (l[1]>y0) && (l[0]<x0+width) && (l[1]<y0+height))
            count++;
        }
      }
      return double(count)/121.0;
    }

#ifdef BIAS_HAVE_XML2
    /** @brief specialization of XML block name function */
    virtual int XMLGetClassName(std::string& TopLevelTag,
                                double& Version) const;

    /** @brief specialization of XML write function */
    virtual int XMLOut(const xmlNodePtr Node, BIAS::XMLIO& XMLObject) const;

    /** @brief specialization of XML read function */
    virtual int XMLIn(const xmlNodePtr Node, BIAS::XMLIO& XMLObject); 
#endif

  protected:
    /// original image position
    BIAS::Vector2<double> t_;
    
    /// "orientation + area(detA) of feature"
    /// affine matrix which transforms all pixels of the unit feature (or 
    /// imagine the local feature coordinate system) of one-pixel square 
    /// into the shape of the current feature(into the image coordinate system)
    /// e.g. y = (1;0) is the 3 o'clock ray to the feature border
    /// x_i = A_ * x_f + t_;
    BIAS::Matrix2x2<double> A_;

    /// 6x6 covariance matrix of the 6 region parameters(a11,a12,a21,a22,tx,ty)
    BIAS::Matrix<double> Cov_;
   
    friend BIASGeometry_EXPORT std::ostream& operator<<(std::ostream& os, 
                                                        const LocalAffineFrame& fp);

    friend BIASGeometry_EXPORT std::istream& operator>>(std::istream& is,
                                                        LocalAffineFrame& fp);

  }; // end class LocalAffineFrame

  /** @relates LocalAffineFrame */
  BIASGeometry_EXPORT std::ostream& operator<<(std::ostream& os,
                                               const LocalAffineFrame& fp);
  
  /** @relates LocalAffineFrame */
  BIASGeometry_EXPORT std::istream& operator>>(std::istream& is, 
                                               LocalAffineFrame& fp);

} // end namespace


#endif