TextureTransformEuclidian.hh

/* 
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003, 2004    (see file CONTACTS for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef __TextureTransformEuclidian__hh__
#define __TextureTransformEuclidian__hh__

#include <Base/Common/BIASpragmaStart.hh>
#include <Image/TextureTransform.hh>
#include <Base/Common/CompareFloatingPoint.hh>

namespace BIAS {

  /** @class TextureTransformEuclidian
   *  @brief analytic properties of Euclidian image warp (rot.+displ.)
   *
   *  See BIAS::TextureTransform for explanation of methods.
   *  @author koeser 04/2008
   **/
  class TextureTransformEuclidian: public TextureTransform {
  public:
    TextureTransformEuclidian():origin_(0.0,0.0) {
      P_.newsize(3);
      P_[0] = 0;
      P_[1] = 0;
      P_[2] = 0;
    }
   
    virtual ~TextureTransformEuclidian(){};

    int MapForward(const HomgPoint2D& src, HomgPoint2D& sink) const {
      const double& sx = src[0];
      const double& sy = src[1];
      sink[0] = A_[0][0]*sx + A_[0][1]*sy + tx_;
      sink[1] = A_[1][0]*sx + A_[1][1]*sy + ty_;
      sink[2] = 1;
      return 0;
    }

    int MapBackward(const HomgPoint2D& sink, HomgPoint2D& src) const {
      src[0] =  Ainv_[0][0]*sink[0] + Ainv_[0][1]*sink[1] + txinv_;
      src[1] =  Ainv_[1][0]*sink[0] + Ainv_[1][1]*sink[1] + tyinv_;
      src[2] = 1;
#ifdef BIAS_DEBUG
      HomgPoint2D testsink;
      MapForward(src, testsink);
      if (!Equal(testsink[0]+1e-4,sink[0]+1e-4, 1e-4) ||  
          !Equal(testsink[1]+1e-4,sink[1]+1e-4, 1e-4)) {
        BIASERR(testsink<<" and "<<sink<<" are not equal with P="<<P_
                <<" and src="<<src<<" and A="<<A_);
        BIASABORT;
      }
#endif
      return 0;
    } 

    int TextureJacobianForward(const HomgPoint2D& src, 
                               Matrix2x2<double>& Jac) const {
      Jac = A_;
      return 0;
    }

    int TextureJacobianBackward(const HomgPoint2D& sink, 
                                Matrix2x2<double>& Jac) const {
      Jac = Ainv_;
      return 0;
    }

    /** @brief local warp is the same at any image position */
    virtual bool TextureJacobianIsConstant() const { return true;};

    int ParameterJacobianForward(Matrix<double>& Jac, const HomgPoint2D& src) {
      Jac.newsize(2,3);

      const double sinphi = sin(P_[0]);
      const double cosphi = cos(P_[0]);
      const double x = src[0]-origin_[0];
      const double y = src[1]-origin_[1];

      Jac[0][0] = -sinphi*x-cosphi*y;
      Jac[0][1] = 1;  
      Jac[0][2] = 0;

      Jac[1][0] = cosphi*x-sinphi*y;
      Jac[1][1] = 0;
      Jac[1][2] = 1;

      return 0;
    }
    
    int ParameterJacobianBackward(Matrix<double>& Jac, const HomgPoint2D& src){
      Jac.newsize(2,3);
      const double sinphi = sin(P_[0]);
      const double cosphi = cos(P_[0]);
      
      const double c13 = P_[1];
      const double c23 = P_[2];
      const double x = src[0]-origin_[0];
      const double y = src[1]-origin_[1];
      // dx/dP_
      Jac[0][0] = -sinphi*x+cosphi*y-cosphi*c23+c13*sinphi; 
      Jac[0][1] = -cosphi; 
      Jac[0][2] = -sinphi;

      // dy/dP_
      Jac[1][0] = -cosphi*x-sinphi*y+sinphi*c23+c13*cosphi;
      Jac[1][1] =  sinphi; 
      Jac[1][2] = -cosphi;

      return 0;
    }

    /** @brief the jacobian depends on x */
    bool ParameterJacobianIsConstant() const { return false;};


    /** @brief rotation(rad), scale-1, dx,dy. All parameters with respect
        to origin_!              x' = A*(x-origin)+d+origin  
        where                  A==(s+1)[cos -sin; sin cos]        
    */
    void SetParameters(const Vector<double>& p) {
      BIASASSERT(p.Size()==3);
      P_ = p;
      A_[0][0] = A_[1][1] = cos(p[0]);
      A_[0][1] = -sin(p[0]);
      A_[1][0] = -A_[0][1];

      Vector2<double> offset(A_*origin_-origin_);
      
      tx_ = p[1]-offset[0];
      ty_ = p[2]-offset[1];

      Ainv_ = A_.Invert();
      txinv_ = -(Ainv_[0][0]*tx_ + Ainv_[0][1]*ty_);
      tyinv_ = -(Ainv_[1][0]*tx_ + Ainv_[1][1]*ty_);
    }

    void ComposeWithInverseDeltaP(const Vector<double>& deltaP) {
      Matrix3x3<double> A1(MatrixIdentity),A2(MatrixIdentity);
      A1[0][0] = A_[0][0];
      A1[0][1] = A_[0][1];
      A1[1][0] = A_[1][0];
      A1[1][1] = A_[1][1];

      A1[0][2] = tx_;
      A1[1][2] = ty_; 

      A2[0][0] = A2[1][1] = cos(deltaP[0]);
      A2[0][1] = -sin(deltaP[0]);
      A2[1][0] = -A2[0][1];

 
      A2[0][2] = deltaP[1]-(A2[0][0]-1.0)*origin_[0]-A2[0][1]*origin_[1];
      A2[1][2] = deltaP[2]-A2[1][0]*origin_[0]-(A2[1][1]-1.0)*origin_[1];

      A1 = A2.Invert() * A1;

      Vector<double> p(3);
      p[0] = P_[0]-deltaP[0];
      p[1] = A1[0][2] + (A1[0][0]-1.0)*origin_[0] + A1[0][1]*origin_[1];
      p[2] = A1[1][2] + A1[1][0]*origin_[0] + (A1[1][1]-1.0)*origin_[1];
      SetParameters(p);
    }

    virtual TextureTransformEuclidian* Clone() const {
      return new TextureTransformEuclidian(*this);
    }

    int ParameterInversionJacobian(Matrix<double>& Jac) const {
      
      Jac.newsize(3,3);
      Jac.SetZero();

      const double cosine = cos(P_[0]);
      const double sine = sin(P_[0]);

      Jac[0][0] = -1.0; // alpha -> -alpha
      
      // tx -> -1/sigma * (cos * tx + sin * ty)
      Jac[1][0] = sine*P_[2]-cosine*P_[2]; 

      Jac[1][1] = -cosine;
      Jac[1][2] = -sine;
      // ty -> +1/sigma * (sin * tx - cos * ty)
      Jac[2][0] = cosine*P_[1]+sine*P_[2]; 

      Jac[2][1] = sine;
      Jac[2][2] = -cosine;

      return 0;
    }

    /** @brief origin relative to which rotation and scale is performed */
    void SetOrigin(const Vector2<double>& origin) {
      origin_ = origin;
      SetParameters(P_);
    }

    /** @brief origin relative to which rotation and scale is performed */
    const Vector2<double>& GetOrigin() const {
      return origin_;
    }

  protected:
    /// cached local warp and inverse
    Matrix2x2<double> A_, Ainv_;
    /// cached displacement and inverse
    double tx_, ty_, txinv_, tyinv_;
    /// origin relative to which rotation and scale is performed
    Vector2<double> origin_;
  }; 

} // end namespace BIAS

#endif