AffineMapping.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/BIASpragma.hh>
#if defined(WIN32) && defined(_MSC_VER)
#  pragma warning (disable: 4661) /* no suitable def. for explicit instantiation */
#endif

#include "AffineMapping.hh"
#include <Base/Geometry/HomgPoint2D.hh>
#include <Geometry/HMatrix.hh> // for comfortable 3x3 mults
#include <Base/Image/ImageIO.hh>
using namespace std;
using namespace BIAS;

// (1.0/log(2.0));
#define ONE_OVER_LOG2 1.442695

template <class InputStorageType, class OutputStorageType>
AffineMapping<InputStorageType, OutputStorageType>::
~AffineMapping() 
{}


template <class InputStorageType, class OutputStorageType>
AffineMapping<InputStorageType, OutputStorageType>::
AffineMapping()
{ SetAffineTransformation(1.0, 0.0, 0.0, 1.0, 0.0, 0.0); }


template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
GetSourceCoordinates_(const HomgPoint2D& sink,
                      HomgPoint2D& source) const {
  BIASASSERT(sink[2]==1.0);
  // compute coordinates in source image with backward tranformation
  source[0] = sink[0] * a11_ + sink[1] * a12_ + dx_;
  source[1] = sink[0] * a21_ + sink[1] * a22_ + dy_;
  return 0;
}

template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
GetJacobian_(const HomgPoint2D& sink, Matrix2x2<double>& Jacobian) const {
  // analytic jacobian of affine transformation IS matrix A
  
  /* 
  // old, slow version:
  Jacobian[0][0] = a11_;
  Jacobian[0][1] = a12_;
  Jacobian[1][0] = a21_;
  Jacobian[1][1] = a22_;
  */

  register double* pJacobian = Jacobian.GetData();
  *pJacobian++ = a11_;
  *pJacobian++ = a12_;
  *pJacobian++ = a21_;
  *pJacobian++ = a22_;
  
  return 0;
}

template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
GetBoundingBox(unsigned int sourcewidth, unsigned int sourceheight,
               unsigned int sinkwidth, unsigned int sinkheight,
               int &tlx,  int &tly, 
               int &brx,  int &bry) {

  // do a forward mapping of the corners of the source image and compute
  // convex hull rectangle

  HomgPoint2D tl(0,0,1), br(sourcewidth, sourceheight,1), bbTL, bbBR;
  HomgPoint2D bl(0,sourceheight,1), tr(sourcewidth,0,1), bbBL, bbTR;
  
  HMatrix A(MatrixIdentity);
  A[0][0] = a11_;
  A[0][1] = a12_;
  A[1][0] = a21_;
  A[1][1] = a22_;
  A[0][2] = dx_;
  A[1][2] = dy_;

  HMatrix HI;
#ifdef BIAS_DEBUG
  BIASASSERT(A.GetInverse(HI)==0);
#else
  A.GetInverse(HI);
#endif
  //HMatrix HI= (Matrix3x3<double>)A.Invert();
  bbTL= HI*tl; bbTL.Homogenize();
  bbBR= HI*br; bbBR.Homogenize();
  bbBL= HI*bl; bbBL.Homogenize();
  bbTR= HI*tr; bbTR.Homogenize();
  //cout<<"corners of src map forward to sink as ";
  //cout<<bbTL<<bbBR<< bbBL<< bbTR<<endl;
  double dtlx,dtly, dbrx,dbry;

  // min(x) and min(y)
  dtlx=(bbTL[0]); 
  if (bbBR[0]<dtlx) dtlx=(bbBR[0]);
  if (bbBL[0]<dtlx) dtlx=(bbBL[0]);
  if (bbTR[0]<dtlx) dtlx=(bbTR[0]);

  dtly=(bbTL[1]); 
  if (bbBR[1]<dtly) dtly=(bbBR[1]);
  if (bbBL[1]<dtly) dtly=(bbBL[1]);
  if (bbTR[1]<dtly) dtly=(bbTR[1]);

  // max(x) and max(y)
  dbrx=(bbTL[0]); 
  if (bbBR[0]>dbrx) dbrx=(bbBR[0]);
  if (bbBL[0]>dbrx) dbrx=(bbBL[0]);
  if (bbTR[0]>dbrx) dbrx=(bbTR[0]);

  dbry=(bbTL[1]); 
  if (bbBR[1]>dbry) dbry=(bbBR[1]);
  if (bbBL[1]>dbry) dbry=(bbBL[1]);
  if (bbTR[1]>dbry) dbry=(bbTR[1]);
   
  // check upper integer limits
  if (dtlx >= double(INT_MAX)) dtlx = INT_MAX;
  if (dtly >= double(INT_MAX)) dtly = INT_MAX;
  if (dbrx>double(INT_MAX)) dbrx = INT_MAX;
  if (dbry>double(INT_MAX)) dbry = INT_MAX;
  // check lower integer limits
  const double minnegativeint = 3.0-double(INT_MAX);
  if (dtlx < minnegativeint) dtlx = minnegativeint;
  if (dtly < minnegativeint) dtly = minnegativeint;
  if (dbrx < minnegativeint) dbrx = minnegativeint;
  if (dbry < minnegativeint) dbry = minnegativeint;
 
  
  tlx = ( int)dtlx; 
  tly = ( int)dtly;
  brx = ( int)ceil(dbrx); 
  bry = ( int)ceil(dbry);
 
  return 0;
}

template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
ComputeScales(double& large, double& smallN, double& cosphi, 
              double& costheta) const { 
  
  Matrix2x2<double> A;
  A[0][0] = a11_;
  A[0][1] = a12_;
  A[1][0] = a21_;
  A[1][1] = a22_;

#ifdef OLD_VERSION
  Vector2<double> vector1(0.0,0.0), vector2(0.0,0.0);
  Matrix2x2<double> T;
  A.GetSystemMatrix(T);
  int numev =  T.EigenvalueDecomposition(ev1, vector1, ev2, vector2);
  double ev1 = sqrt(ev1);
  double ev2 = sqrt(ev2);

  if (numev!=2) {
    BIASERR("A="<<A<<" has only rank "<<numev<<". Cannot map.");
    return -1;
  }  
  cosphi = (fabs(ev1)>fabs(ev2))? vector1[0] : vector2[0];
  // upper bound
  costheta = sqrt(0.5);
#else 
  Matrix2x2<double> phi;
  Matrix2x2<double> theta;
double ev1,ev2;
  HMatrix::FactorizeAffineMatrixRLeft(A, phi, theta, ev1, ev2);
  cosphi = 0.5*(phi[0][0]+phi[1][1]);
  costheta = 0.5*(theta[0][0]+theta[1][1]);
#endif
  //cout<<"eigenvalues of A are "<<ev1<<" and "<<ev2<<endl;
  smallN = fabs((fabs(ev1)>fabs(ev2))?ev2:ev1);
  if (smallN<1.0) smallN = 1.0;
    
  large = fabs((fabs(ev1)<fabs(ev2))?ev2:ev1);
  if (large<1.0) {
    large = 1.0;
  }
 
  return 0;
} 




template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
TrilinearGrey(const Image<InputStorageType>& src, 
              Image<OutputStorageType>& sink, double scale) {
  //cout<<"TRILINEAR GREY"<<endl;
  if (scale<=0.0) {
    double smallN, cosphi, costheta;
    ComputeScales(scale, smallN, cosphi, costheta);
  } 
  if (this->autoPyramidSize_)
    this->UpdatePyramidSize(*sink.GetROI(), src.GetWidth(), src.GetHeight());
  this->BuildPyramid_(src, true);
  return TrilinearGreyAgain(sink, scale);
}


template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
TrilinearGreyAgain(Image<OutputStorageType>& sink, double scale) {
  BIASASSERT(!this->pyramid_.empty());
  int tlx=0, tly=0, brx=0, bry=0;
  GetBoundingBox(this->pyramid_[0]->GetWidth(),
                 this->pyramid_[0]->GetHeight(), sink.GetWidth(),
                 sink.GetHeight(), tlx, tly, brx, bry);
  this->ClipBoundingBoxToROICorners_(sink, tlx, tly, brx, bry);
  
  double logsmallestscale = log(scale)*ONE_OVER_LOG2;

  int scale1 = int(logsmallestscale);
  double scale_error = logsmallestscale - scale1;
  
  if (scale1>(int)this->pyramid_.size()-2) {
    // we might get aliasing, pyramid is too small !!!
    scale1 = this->pyramid_.size()-2;
    scale_error = 1.0;
    this->aliasing_ = true;
  } else if (scale1<0) {
    // use max resolution image, dont need to upsample in pyramid
    scale1 = 0;
    scale_error = 0.0;
  }
  const int scale2 = scale1 + 1;
  const Image<InputStorageType> &p1 = *this->pyramid_[scale1], 
    &p2=*this->pyramid_[scale2];
  
  const double weight1 = 1.0 - scale_error, weight2 = scale_error;

  const double offset =this->pyramid_.GetPositionOffset();
  double scaleposition = 1.0, offsetposition = 0.0;
  
  for (int s=0; s<scale1; s++) {
    // -0.5 necessary because of offset in 2^n downsampling
    scaleposition *= 0.5;
    offsetposition += scaleposition * offset;
  }
 
  const double a11 = scaleposition * a11_;
  const double a12 = scaleposition * a12_;
  const double a21 = scaleposition * a21_;
  const double a22 = scaleposition * a22_;
  const double dx = scaleposition * dx_ - offsetposition;
  const double dy = scaleposition * dy_ - offsetposition;
  OutputStorageType* pO = sink.GetImageData()+tly*sink.GetWidth();
  for (unsigned int y=tly; y<sink.GetHeight(); y++) {
    for (unsigned int x=0; x<sink.GetWidth(); x++) {
      // compute coordinates in smaller image
      const double x1 = a11*x + a12*y + dx;
      const double y1 = a21*x + a22*y + dy;  
      if (p1.IsPositionInImage(int(x1-2.001), int(y1-2.001)) && 
          p1.IsPositionInImage(int(x1+2.501), int(y1+2.501))) {
                 
        // better use output storagetype with clipvalue_ here (slower)
        *pO++ = 
          OutputStorageType(weight1 * p1.FastBilinearInterpolationGrey(x1,y1) +
                            weight2 * 
                            p2.FastBilinearInterpolationGrey((x1-offset)*0.5,
                                                             (y1-offset)*0.5));
      } else *pO++ = 0;
    }
  }

  return 0;
}


template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
BilinearGrey(const Image<InputStorageType>& src, 
             Image<OutputStorageType>& sink) { 
  //cout<<"BILINEAR GREY"<<endl;
  int tlx=0, tly=0, brx=0, bry=0;
  GetBoundingBox(src.GetWidth(), src.GetHeight(), sink.GetWidth(),
                 sink.GetHeight(), tlx, tly, brx, bry);
  this->ClipBoundingBoxToROICorners_(sink, tlx, tly, brx, bry);
  
  double destx = 0, desty = 0;
  OutputStorageType *pSink=sink.GetImageData()+tly*sink.GetWidth();
  // run over sink and bilinear interpolate from temp using combined transf.



  // we might update this to use only ROI in sink. However, then simple pointer
  // arithmetics cannot be used. I guess this pays only if a third of
  // sink stays empty



  for (unsigned int y=tly; y<sink.GetHeight(); y++) {
    for (unsigned int x=0; x<sink.GetWidth(); x++) {
      destx = a11_ * x + a12_ * y + dx_;
      desty = a21_ * x + a22_ * y + dy_;
      // use imagedatapointer here
      if (src.IsPositionInImage(int(destx-1.1), int(desty-1.1)) &&
          src.IsPositionInImage(int(destx+1.1), int(desty+1.1)))
        *pSink++ = 
          OutputStorageType(src.FastBilinearInterpolationGrey(destx,
                                                              desty));
      else *pSink++ = 0;
    }
  }

  return 0;
}


template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
MapDirect(const Image<InputStorageType>& src, 
          Image<OutputStorageType>& sink) {

  if (this->autoPyramidSize_)
    this->UpdatePyramidSize(*sink.GetROI(),
                            (int)src.GetWidth(), (int)src.GetHeight());
  this->BuildPyramid_(src);
  return MapDirectAgain(sink);
}

template <class InputStorageType, class OutputStorageType>
int AffineMapping<InputStorageType, OutputStorageType>::
  MapDirectAgain(Image<OutputStorageType>& sink) {
  BIASASSERT(!this->pyramid_.empty());
#ifdef BIAS_DEBUG
  //this->pyramid_.WriteImages("pyr");
#endif
  BIASASSERT(sink.GetChannelCount()==1); // only grey for speed
  
  Matrix3x3<double> A3(MatrixIdentity), Intermediate(MatrixIdentity);
  A3[0][0] = a11_;
  A3[0][1] = a12_;
  A3[1][0] = a21_;
  A3[1][1] = a22_;
  A3[0][2] = dx_;
  A3[1][2] = dy_;
  
  double smallestscale = 0, largescale=0, cosphi = 0, costheta = 0;
  if (ComputeScales(largescale, smallestscale, cosphi, costheta)!=0)
    return -1; 

  if (largescale<1.1) {
    // bilinear is sufficient
    return BilinearGrey(*this->pyramid_[0], sink); 
  }

  const double smoothsigma = 
    //sqrt(largescale*largescale-smallestscale*smallestscale);
    sqrt(0.25*(largescale/smallestscale*largescale/smallestscale)-0.25);
  //cout<<"smoothsigma would be "<<smoothsigma<<endl;
  if (smoothsigma<0.3) {
    // trilinear is sufficient
    return TrilinearGreyAgain(sink, largescale); 
  } else if (smoothsigma>7.5) {
    // we use a hws=5 kernel, the cut-off filter coefficient 6 has more than
    // 10% weight, leaving it away (as we do) is no good idea.
    this->aliasing_ =true;
    BIASWARN("You may have (directional) aliasing in your image. sigma="
             <<smoothsigma);
    // you might try trilinear, this avoids aliasing but oversmoothes image
    // content. Or bigger kernel (manual+slow)...
  }
  //cout<<"ANISOTROPIC GREY"<<endl;
  //cout<<"smallestscale is "<<smallestscale<<" smoothsigma is "<<smoothsigma
  //    <<endl;
  // --------------------------------------------------------------------
  // ----------------- find roi in src image ----------------------------
  // --------------------------------------------------------------------
  const double cosinephi = cosphi;
  const double sinephi = sqrt(1.0-cosinephi*cosinephi);
  const double cosinetheta = costheta;
  const double sinetheta = sqrt(1.0-cosinetheta*cosinetheta);
  //cout<<"cosinephi is "<<cosinephi<<", sinephi is "<<sinephi<<endl;
  int maxx = -1000000, maxy=-1000000, minx=1000000, miny=1000000;
 
  int tlx=0, tly=0, brx=0, bry=0;
  GetBoundingBox(this->pyramid_[0]->GetWidth(), 
                 this->pyramid_[0]->GetHeight(),
                 sink.GetWidth(),
                 sink.GetHeight(), tlx, tly, brx, bry);
  this->ClipBoundingBoxToROICorners_(sink, tlx, tly, brx, bry);

  HomgPoint2D  result(0,0,1), sinkpoint(tlx,tly,1);
  HomgPoint2D CoG(0,0,0);
  for (unsigned int corner=0; corner<4; corner++) {
    switch (corner) {
    case 0: sinkpoint[0] = tlx; sinkpoint[1] = tly; break;
    case 1: sinkpoint[0] = brx; sinkpoint[1] = tly; break;
    case 2: sinkpoint[0] = brx; sinkpoint[1] = bry; break;
    case 3: sinkpoint[0] = tlx; sinkpoint[1] = bry; break;
    }
    if (GetSourceCoordinates_(sinkpoint, result)!=0) continue;
    //cout<<"mapped "<<sinkpoint<<" to "<<result<<endl;
    
    if (int(rint(result[0]))>maxx) maxx = int(rint(result[0]));
    if (int(rint(result[0]))<minx) minx = int(rint(result[0]));
    if (int(rint(result[1]))>maxy) maxy = int(rint(result[1]));
    if (int(rint(result[1]))<miny) miny = int(rint(result[1]));
    
  }
  
  //  if (maxx>=(int)this->pyramid_[0]->GetWidth()) 
  //  maxx = this->pyramid_[0]->GetWidth()-1;
  //if (maxy>=(int)this->pyramid_[0]->GetHeight()) 
  //  maxy = this->pyramid_[0]->GetHeight()-1;
  //if (minx<0) minx = 0;
  //if (miny<0) miny = 0;
  // center of gravity for rotation
  double rotx = 0.5*(minx + maxx);
  double roty = 0.5*(miny + maxy);
  //cout<<"rot center is "<<rotx<<";"<<roty<<endl;
  // --------------------------------------------------------------------
  // ----------------- compute dimensions of tmp ------------------------
  // --------------------------------------------------------------------
  maxx = -1000000, maxy=-1000000, minx=1000000, miny=1000000;
 
  for (unsigned int corner=0; corner<4; corner++) {
    switch (corner) {
    case 0: sinkpoint[0] = tlx; sinkpoint[1] = tly; break;
    case 1: sinkpoint[0] = brx; sinkpoint[1] = tly; break;
    case 2: sinkpoint[0] = brx; sinkpoint[1] = bry; break;
    case 3: sinkpoint[0] = tlx; sinkpoint[1] = bry; break;
    }
    if (GetSourceCoordinates_(sinkpoint, result)!=0) continue;
    sinkpoint = result;
    result[0] = cosinetheta * (sinkpoint[0]-rotx) + 
      sinetheta * (sinkpoint[1]-roty)+rotx;
    result[1] = -sinetheta * (sinkpoint[0]-rotx) + 
      cosinetheta *(sinkpoint[1]-roty)+roty;
    if (int(rint(result[0]))>maxx) maxx = int(rint(result[0]));
    if (int(rint(result[0]))<minx) minx = int(rint(result[0]));
    if (int(rint(result[1]))>maxy) maxy = int(rint(result[1]));
    if (int(rint(result[1]))<miny) miny = int(rint(result[1]));

  }
  for (unsigned int corner=0; corner<4; corner++) {
    switch (corner) {
    case 0: sinkpoint[0] = tlx; sinkpoint[1] = tly; break;
    case 1: sinkpoint[0] = brx; sinkpoint[1] = tly; break;
    case 2: sinkpoint[0] = brx; sinkpoint[1] = bry; break;
    case 3: sinkpoint[0] = tlx; sinkpoint[1] = bry; break;
    }
    if (GetSourceCoordinates_(sinkpoint, result)!=0) continue;
    sinkpoint = result;
    result[0] = cosinephi * (sinkpoint[0]-rotx) + 
      sinephi * (sinkpoint[1]-roty)+rotx;
    result[1] = -sinephi * (sinkpoint[0]-rotx) + 
      cosinephi *(sinkpoint[1]-roty)+roty;
    if (int(rint(result[0]))>maxx) maxx = int(rint(result[0]));
    if (int(rint(result[0]))<minx) minx = int(rint(result[0]));
    if (int(rint(result[1]))>maxy) maxy = int(rint(result[1]));
    if (int(rint(result[1]))<miny) miny = int(rint(result[1]));

  }

  // if ellipse is rotated by 45 degrees, bounding box increases
  //double enlargefactor = fabs(costheta)+sqrt(1-(costheta*costheta));

  Image<InputStorageType> axisaligned(int((maxx-minx+1)/smallestscale+1),
                                      int((maxy-miny+1)/smallestscale+1), 1), 
    filtered(int((maxx-minx+1)/smallestscale+1), 
             int((maxy-miny+1)/smallestscale+1), 1);

  // --------------------------------------------------------------------
  // ---------------- fill smoothing-aligned tmp ------------------------
  // --------------------------------------------------------------------
  
  Intermediate[0][0] = cosinephi;
  Intermediate[0][1] = -sinephi;
  Intermediate[1][0] = sinephi;
  Intermediate[1][1] = cosinephi;
  Intermediate[0][2] = cosinephi*(axisaligned.GetWidth()-1.0)/-2.0 + 
    sinephi*(axisaligned.GetHeight()-1.0)/2.0;
  Intermediate[1][2] = sinephi*(axisaligned.GetWidth()-1.0)/-2.0 +
    cosinephi * (axisaligned.GetHeight()-1.0)/-2.0;
  Intermediate *= smallestscale;
  Intermediate[2][2] = 1;
  Intermediate[0][2]+=rotx;
  Intermediate[1][2]+=roty;

  
 
  //cout<<"intermediate is "<<Intermediate<<endl;
  double logsmallestscale = log(smallestscale)*ONE_OVER_LOG2;

  int scale1 = int(logsmallestscale);
  double scale_error = logsmallestscale - scale1;
  
  if (scale1>(int)this->pyramid_.size()-2) {
    // we might get aliasing, pyramid is too small !!!
    scale1 = this->pyramid_.size()-2;
    scale_error = 1.0;
    this->aliasing_ = true;
  } else if (scale1<0) {
    // use max resolution image, dont need to upsample in pyramid
    scale1 = 0;
    scale_error = 0.0;
  }
  const int scale2 = scale1 + 1;
  const Image<InputStorageType> &p1 = *this->pyramid_[scale1], 
    &p2=*this->pyramid_[scale2];
  
  const double weight1 = 1.0 - scale_error, weight2 = scale_error;

  const double offset =this->pyramid_.GetPositionOffset();
  double scaleposition = 1.0, offsetposition = 0.0;
  for (int s=0; s<scale1; s++) {
    // -0.5 necessary because of offset in 2^n downsampling
    offsetposition += scaleposition * offset;
    scaleposition *= 0.5;
  }

  const double i00 = Intermediate[0][0], i01=Intermediate[0][1],
    i02=Intermediate[0][2];
  const double i10 = Intermediate[1][0], i11=Intermediate[1][1],
    i12=Intermediate[1][2];
  
  InputStorageType* pI = axisaligned.GetImageData();
  for (unsigned int y=0; y<axisaligned.GetHeight(); y++) {
    for (unsigned int x=0; x<axisaligned.GetWidth(); x++) {
      // compute coordinates in smaller image
      const double x1 = scaleposition * (i00*x + i01*y + i02) + offsetposition;
      const double y1 = scaleposition * (i10*x + i11*y + i12) + offsetposition;
         
      if (p1.IsPositionInImage(int(x1-2.1), int(y1-2.1)) && 
          p1.IsPositionInImage(int(x1+2.6), int(y1+2.6))) {
                 
        // better use output storagetype with clipvalue_ here
        *pI++ = 
          InputStorageType(weight1 * p1.FastBilinearInterpolationGrey(x1,y1) +
                           weight2 * 
                           p2.FastBilinearInterpolationGrey((x1-offset)*0.5,
                                                            (y1-offset)*0.5));
      } else *pI++ = 0;
    }
  }
#ifdef BIAS_DEBUG
  //ImageIO::Save("B.mip", axisaligned);
#endif
  // --------------------------------------------------------------------
  // -------------------- filter horizontal -----------------------------
  // --------------------------------------------------------------------
  
  // create horizontal gauss mask with smooth sigma and run over temp
  const double fac = -1.0/(2.0*smoothsigma);
  
  double h0 = 1.0;
  double h1 = expf(fac);
  double h2 = expf(4.0*fac);
  double h3 = expf(9.0*fac);
  double h4 = expf(16.0*fac);
  double h5 = expf(25.0*fac);
  double sum = 1.0 /(h0 + 2.0*(h1+h2+h3+h4+h5));
  h0 *= sum; h1 *= sum; h2 *= sum; h3 *= sum; h4 *= sum; h5 *= sum;
 

  // horizontal convolution ignoring wrap-arounds
  InputStorageType *pS  = axisaligned.GetImageData(), 
    *pD = filtered.GetImageData();

  // copy first five bytes
  *pD++ = *pS++;
  *pD++ = *pS++;
  *pD++ = *pS++;
  *pD++ = *pS++;
  *pD++ = *pS;

  const int width = axisaligned.GetWidth(), height = axisaligned.GetHeight();
 
  const InputStorageType* pEndH = axisaligned.GetImageData() + width*height-5;
  while (++pS<pEndH) {
    *pD++ = InputStorageType(h0*pS[0]+h1*(pS[-1]+pS[1])+h2*(pS[-2]+pS[2])
                             +h3*(pS[-3]+pS[3])+h4*(pS[-4]+pS[4])+h5*(pS[-5]+pS[5]));
  }
  // copy last five bytes
  *pD++ = *pS++;
  *pD++ = *pS++;
  *pD++ = *pS++;
  *pD++ = *pS++;
  *pD   = *pS;

#ifdef BIAS_DEBUG
  //ImageIO::Save("Bfiltered.mip", filtered);
#endif

  // link A and scale and rotation (eliminate offset ?)
  //   Matrix3x3<double> direct(Intermediate.GetInvert() * A3);
  Matrix3x3<double> direct;
#ifdef BIAS_DEBUG
  BIASASSERT(Intermediate.GetInverse(direct) == 0);
#else
  Intermediate.GetInverse(direct);
#endif
  direct *= A3;

  direct *= 1.0 / direct[2][2]; // homogenize
  const double d00=direct[0][0], d01=direct[0][1], d02=direct[0][2];
  const double d10=direct[1][0], d11=direct[1][1], d12=direct[1][2];

  double destx = 0, desty = 0;
  OutputStorageType *pSink=sink.GetImageData()+tly*sink.GetWidth();
  // run over sink and bilinear interpolate from temp using combined transf.

  // compute all sink pixels here or use roi (cannot use pointer psink++ then)?

  for (unsigned int y=tly; y<sink.GetHeight(); y++) {
    for (unsigned int x=0; x<sink.GetWidth(); x++) {
      destx = d00 * x + d01 * y + d02;
      desty = d10 * x + d11 * y + d12;
      // use imagedatapointer here
      if (filtered.IsPositionInImage(int(destx-1.1), int(desty-1.1)) &&
          filtered.IsPositionInImage(int(destx+1.1), int(desty+1.1)))
        *pSink++ = 
          OutputStorageType(filtered.FastBilinearInterpolationGrey(destx,
                                                                   desty));
      else *pSink++ = 0;
    }
  }

  return 0;
} 







namespace BIAS{
template class AffineMapping<unsigned char, unsigned char>;
template class AffineMapping<float, float>;
template class AffineMapping<unsigned char, float>;
template class AffineMapping<float, unsigned char>;

#if defined(BUILD_IMAGE_CHAR)
template class AffineMapping<unsigned char, char>;
template class AffineMapping<char, char>;
#endif

#if defined(BUILD_IMAGE_USHORT)
template class AffineMapping<unsigned short,unsigned short>;
#endif

#if defined(BUILD_IMAGE_SHORT)
template class AffineMapping<short, short>;
#endif

#if defined(BUILD_IMAGE_SHORT)&&defined(BUILD_IMAGE_USHORT)
template class AffineMapping<unsigned short, short>;
#endif

#if defined(BUILD_IMAGE_INT)
template class AffineMapping<int,int>;
#endif

#if defined(BUILD_IMAGE_USHORT)
template class AffineMapping<unsigned short,float>;
#endif

#if defined(BUILD_IMAGE_USHORT) && defined(BUILD_IMAGE_INT)
template class AffineMapping<unsigned short,int>;
#endif

#if defined(BUILD_IMAGE_DOUBLE)
template class AffineMapping<double,double>;
#endif
}