TextureTransformSimilar.hh

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

Copyright (C) 2003-2009    (see file CONTACT 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 __TextureTransformSimilar__hh__
#define __TextureTransformSimilar__hh__

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

namespace BIAS {

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

    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!!!");
        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,4);

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

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

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

      return 0;
    }
    
    int ParameterJacobianBackward(Matrix<double>& Jac, const HomgPoint2D& src){
      Jac.newsize(2,4);
      const double sinphi = sin(P_[0]);
      const double cosphi = cos(P_[0]);
      const double scale = P_[1];
      const double scaleplus1 = scale+1.0;
      const double c13 = P_[2];
      const double c23 = P_[3];
      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)/scaleplus1; 
      Jac[0][1] = (-cosphi*x-sinphi*y+sinphi*c23+c13*cosphi)/
        (scaleplus1*scaleplus1);
      Jac[0][2] = -cosphi/scaleplus1; 
      Jac[0][3] = -sinphi/scaleplus1;

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

      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()==4);
      P_ = p;
      A_[0][0] = A_[1][1] = cos(p[0])*(p[1]+1.0);
      A_[0][1] = -sin(p[0])*(p[1]+1.0);
      A_[1][0] = -A_[0][1];

      Vector2<double> offset(A_*origin_-origin_);
      
      tx_ = p[2]-offset[0];
      ty_ = p[3]-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])*(deltaP[1]+1.0);
      A2[0][1] = -sin(deltaP[0])*(deltaP[1]+1.0);
      A2[1][0] = -A2[0][1];

 
      A2[0][2] = deltaP[2]-(A2[0][0]-1.0)*origin_[0]-A2[0][1]*origin_[1];
      A2[1][2] = deltaP[3]-A2[1][0]*origin_[0]-(A2[1][1]-1.0)*origin_[1];
      Matrix3x3<double> A2inv;
      A2.GetInverse(A2inv);
      A1 = A2inv * A1;

      Vector<double> p(4);
      p[1] = sqrt(A1.GetDeterminant());
      const double sine = (A1[1][0]-A1[0][1])/(2.0*p[1]);
      const double cosine = (A1[0][0]+A1[1][1])/(2.0*p[1]);
      p[0] = atan2(sine, cosine);
      p[1] -= 1.0;
      p[2] = A1[0][2] + (A1[0][0]-1.0)*origin_[0] + A1[0][1]*origin_[1];
      p[3] = A1[1][2] + A1[1][0]*origin_[0] + (A1[1][1]-1.0)*origin_[1];
      SetParameters(p);
    }


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

    /** @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