TextureTransformAffine.cpp

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

#include <Base/Common/BIASpragmaStart.hh>
#include <Image/TextureTransformAffine.hh>
#include <MathAlgo/SVD.hh>
#include <iostream>

using namespace std;



#define MIN_DET 1e-6

int BIAS::TextureTransformAffine::
ParameterInversionJacobian(Matrix<double>& Jac) const {

  Jac.newsize(6,6);
  double * pD = Jac.GetData();

  const double a11 = P_[0];
  const double a12 = P_[1];
  const double a21 = P_[2];
  const double a22 = P_[3];
  const double tx  = P_[4];
  const double ty  = P_[5];


  const double det = a11*a22+a11+a22+1.0-a12*a21;
  if (fabs(det)<MIN_DET) {
    // we have one thousand/million times minification, degenerate case !
    return -1;
  }
  const double detdet = det * det;

  *pD++ = -(a22+1.0)*(a22+1.0)/detdet;    
  *pD++ =  (a22+1.0)/detdet*a21;
  *pD++ =  (a22+1.0)/detdet*a12;
  *pD++ = -a12/detdet*a21;
  *pD++ =  0; 
  *pD++ =  0;

  *pD++ =  (a22+1.0)/detdet*a12;
  *pD++ = -(a11*a22+a11+a22+1.0)/detdet;
  *pD++ = -a12*a12/detdet;
  *pD++ =  a12/detdet*(a11+1.0);
  *pD++ =  0;
  *pD++ =  0;

  *pD++ =  (a22+1.0)/detdet*a21;
  *pD++ = -a21*a21/detdet; 
  *pD++ = -(a11*a22+a11+a22+1.0)/detdet;
  *pD++ =  a21/detdet*(a11+1.0);
  *pD++ =  0;
  *pD++ =  0;

  *pD++ = -a12/detdet*a21;
  *pD++ =  a21/detdet*(a11+1.0);  
  *pD++ =  a12/detdet*(a11+1.0); 
  *pD++ = -(a11+1.0)*(a11+1.0)/detdet;
  *pD++ =  0;
  *pD++ =  0;

  *pD++ =  (a22+1.0)*(tx*a22+tx-a12*ty)/detdet;
  *pD++ = -(tx*a21*a22+tx*a21-ty*a11*a22-ty*a11-ty*a22-ty)/detdet;
  *pD++ = -a12*(tx*a22+tx-a12*ty)/detdet;
  *pD++ =  a12*(tx*a21-ty*a11-ty)/detdet;
  *pD++ = -(a22+1.0)/det;
  *pD++ =  a12/det;

  *pD++ = -a21*(tx*a22+tx-a12*ty)/detdet;
  *pD++ =  a21*(tx*a21-ty*a11-ty)/detdet;
  *pD++ =  (tx*a11*a22+tx*a11+tx*a22+tx-a12*ty*a11-a12*ty)/detdet;
  *pD++ = -(a11+1.0)*(tx*a21-ty*a11-ty)/detdet;
  *pD++ =  a21/det;
  *pD++ = -(a11+1.0)/det;
 
                                           
  return 0;
}



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

int BIAS::TextureTransformAffine::
MapBackward(const HomgPoint2D& sink, HomgPoint2D& src) const 
{
  src[0] = sink[0]*Ainv_[0][0] + sink[1]*Ainv_[0][1] + txinv_;
  src[1] = sink[0]*Ainv_[1][0] + sink[1]*Ainv_[1][1] + tyinv_;
  src[2] = 1;
  return 0;
} 

  
int BIAS::TextureTransformAffine::
ParameterJacobianForward(Matrix<double>& Jac, const HomgPoint2D& src) {
      

  Jac.newsize(2,6);
  Jac.SetZero();
  Jac[0][0] = src[0]-origin_[0]; 
  Jac[0][1] = src[1]-origin_[1];
  Jac[0][4] = 1;
  Jac[1][2] = src[0]-origin_[0];
  Jac[1][3] = src[1]-origin_[1];
  Jac[1][5] = 1;
  return 0;
}
    
int BIAS::TextureTransformAffine::
ParameterJacobianBackward(Matrix<double>& Jac, const HomgPoint2D& sink){
  Jac.newsize(2,6);
  Jac.SetZero();
  //cout<<"Jacobian evaluated at "<<sink<<endl;
  const double c11 = P_[0];
  const double c12 = P_[1];
  const double c21 = P_[2];
  const double c22 = P_[3];
  const double c13 = P_[4];
  const double c23 = P_[5];
  const double det = c11*c22+c11+c22+1-c21*c12;
  if (fabs(det)<MIN_DET) {
    // det is too small to make sense
    return -1;
  }
  const double detdet = det*det;
  const double x = sink[0]-origin_[0]-P_[4];
  const double y = sink[1]-origin_[1]-P_[5];
  //  cout<<"diff from transferred origin is "<<x<<";"<<y<<endl;
  Jac[0][0] =  -(c22+1)*(x*c22+x-c12*y+c12*c23-c13*c22-c13)/detdet;
  Jac[0][1] =  (x*c21*c22 + x*c21 - y*c11*c22 - y*c11 - y*c22 - y + 
                c23*c11*c22 + c23*c11 + c23*c22 + c23 - c13*c21*c22 - 
                c13*c21)/detdet;
       
  Jac[0][2] =  c12*(x*c22+x-c12*y+c12*c23-c13*c22-c13)/detdet;
  Jac[0][3] =  -c12*(x*c21-y*c11-y+c23*c11+c23-c13*c21)/detdet;
  Jac[0][4] =  -(c22+1)/det;       
  Jac[0][5] =  c12/det;

  Jac[1][0] =    c21*(x*c22+x-c12*y+c12*c23-c13*c22-c13)/detdet;
  Jac[1][1] = -c21*(x*c21-y*c11-y+c23*c11+c23-c13*c21)/detdet; 
  Jac[1][2] = -(x*c11*c22 + x*c11 + x*c22 + x -c12*y*c11 -c12*y -
                c13*c11*c22 - c13*c11 - c13*c22 - c13 + c12*c23*c11
                + c12*c23)/detdet;
  Jac[1][3] = (c11+1)*(x*c21-y*c11-y+c23*c11+c23-c13*c21)/detdet;
  Jac[1][4] = c21/det;
  Jac[1][5] = -(c11+1)/det;

  return 0;
}

void BIAS::TextureTransformAffine::
SetParameters(const Vector<double>& p) {
  BIASASSERT(p.Size()==6);
#ifdef BIAS_DEBUG
  for (unsigned int i=0; i<6; i++) {
    INFNANCHECK(p[i]);
  }
#endif
  P_ = p;
  InterpretParameters(p, A_[0][0], A_[0][1], A_[1][0], A_[1][1], tx_, ty_);
  
  if (A_.Invert( Ainv_)!=0) {
    BIASERR("Error inverting A_="<<A_<<" with det "<<A_.det());
    Ainv_.SetZero();
  }
  txinv_ = -(Ainv_[0][0]*tx_ + Ainv_[0][1]*ty_);
  tyinv_ = -(Ainv_[1][0]*tx_ + Ainv_[1][1]*ty_);
}


void BIAS::TextureTransformAffine::
ComposeWithInverseDeltaP(const Vector<double>& deltaP) {
#ifdef BIAS_DEBUG
  for (unsigned int i=0; i<6; i++) {
    INFNANCHECK(deltaP[i]);
  }
#endif
  Matrix3x3<double> A1(MatrixZero), A2inv;
  A1[2][2] = 1.0;
  InterpretParameters(deltaP, A1[0][0],  A1[0][1],  A1[1][0],  A1[1][1], 
                      A1[0][2],  A1[1][2]);
  if (A1.GetInverse(A2inv)!=0) {
    BIASERR("Error inverting warp "<<A1);
    // try pseudoinverse
    Matrix<double> m(A1); // convert to Matrix
    SVD mysvd(m);
    A2inv = mysvd.Invert();
  }

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

  A1 = A2inv * A1;

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