TextureTransformRotation.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 __TextureTransformRotation__hh__
#define __TextureTransformRotation__hh__

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

namespace BIAS {

  /** @class TextureTransformRotation
   *  @brief analytic properties of rotational image warp (around some point)
   *
   *  See BIAS::TextureTransform for explanation of methods.
   *  @author koeser 04/2008
   **/
  class TextureTransformRotation: public TextureTransform {
  public:
    TextureTransformRotation():origin1_(0.0,0.0) {
      P_.newsize(1);
      P_[0] = 0;
    }
   
    virtual ~TextureTransformRotation(){};

    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,1);

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

      Jac[0][0] = -sinphi*x-cosphi*y;
      Jac[1][0] = cosphi*x-sinphi*y;
     
      return 0;
    }
    
    int ParameterJacobianBackward(Matrix<double>& Jac, const HomgPoint2D& src){
      Jac.newsize(2,1);
      const double sinphi = sin(P_[0]);
      const double cosphi = cos(P_[0]);
     
      const double x = src[0]-origin1_[0];
      const double y = src[1]-origin1_[1];
      // dx/dP_ ( dcos(x)/dx = -sin, therefore sin and cos appear swapped)
      Jac[0][0] = -sinphi*x+cosphi*y; 
      
      // dy/dP_
      Jac[1][0] = -cosphi*x-sinphi*y;
     
      return 0;
    }

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


    /** @brief rotation(rad) with respect to origin_!
     *         x' = A*(x-origin)+origin  
        where       A==[cos -sin; sin cos]        
    */
    void SetParameters(const Vector<double>& p) {
      BIASASSERT(p.Size()==1);
      P_ = p;
      Ainv_[0][0] = Ainv_[1][1] = A_[0][0] = A_[1][1] = cos(p[0]);
      Ainv_[1][0] = A_[0][1] = -sin(p[0]);
      Ainv_[0][1] = A_[1][0] = -A_[0][1];
      
      Vector2<double> offset(A_*origin1_-origin1_);
      
      tx_ = -offset[0];
      ty_ = -offset[1];

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

    void ComposeWithInverseDeltaP(const Vector<double>& deltaP) {
      Vector<double> p(1);
      p[0] = P_[0] - deltaP[0];
      SetParameters(p);
    }

    int ParameterInversionJacobian(Matrix<double>& Jac) const {
      // inverse is simply inverse angle:   alpha -> -alpha
      Jac.newsize(1,1);
      Jac[0][0] = -1;
      return 0;
    }

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

    /** @brief set origin in image1 and image2 */
    void SetOrigin(const Vector2<double>& origin1) {
      origin1_ = origin1;
      SetParameters(P_);
    }

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

  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> origin1_;
  }; 

} // end namespace BIAS

#endif