ImageAlignment.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 <Matcher2D/ImageAlignment.hh>
#include <MathAlgo/SVD.hh>
#include <Base/Image/ImageIO.hh> // only for debug image output
#include <Base/Common/FileHandling.hh>
#include <Base/Common/BIASpragma.hh>

// use isotropic or anisotropic parameter uncertainty
//#define  ANISOTROPIC


// if grey value cannot be obtained, which value is set
#define UNDEF_VALUE 0.0

using namespace BIAS;
using namespace std;

BIAS::ImageAlignment::ImageAlignment():
  MaxIterations_(20), MaxUpdate_(0.01), Dampening_(0.0),
  AffineBrightnessInvariance_(false),
  LineSearch_(0.3), intensitySigma_(1.0),
  T_(NULL), mean1_(0.0),
  mean2_(0.0), dev1_(1.0), dev2_(1.0),iterationDebug_(0) {

}


int BIAS::ImageAlignment::
Align(const PyramidImage<float>& I1, const PyramidImage<float>& I2,
      Vector<double>& params, Matrix<double>& Cov, int PyramidLevel)
{
  // make sure the user has set a warp model
  BIASASSERT(T_!=NULL);

  int status = 0;
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
           "compute template and gradients from warped I1 with prediction "
           <<params);
  T_->SetParameters(params);

  Matrix<double> PIJ;
  if (T_->ParameterInversionJacobian(PIJ)!=0) return -3;
  const Matrix<double> CovInv(PIJ * Cov * PIJ.Transpose());

  // check whether we have to re-evaluate for each pixel:
  const bool TextureJacobianIsConstant = T_->TextureJacobianIsConstant();
  const bool ParameterJacobianIsConstant = T_->ParameterJacobianIsConstant();

  // reset active flag
  for (unsigned int i=0; i<active_.size(); i++) active_[i] = true;


  if (PyramidLevel<0) {
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
             "auto pyramid, determine required scale for each pixel");
    Matrix<double> JacParams;
    Matrix2x2<double> JacTex;
#ifdef BIAS_DEBUG
    Image<float> *ptheweights = NULL;
    float *pwdata = NULL;
    // save debug image with weights
    if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES)) {
      int numberOfPixelsPerRow =
        int(ceil(sqrt(double(TemplatePositions_.size()))));
#ifdef ANISOTROPIC
      ptheweights = new Image<float>(numberOfPixelsPerRow,
                                     numberOfPixelsPerRow,3);
#else
      ptheweights = new Image<float>(numberOfPixelsPerRow,
                                     numberOfPixelsPerRow,1);
#endif
      ptheweights->SetZero();
      pwdata = ptheweights->GetImageData();
    }
#endif
    for (unsigned int p=0; p<TemplatePositions_.size(); p++) {
      // which pixel in the source image ?
      const HomgPoint2D& x(TemplatePositions_[p]);
      // where does the prediction map it ?
      T_->MapForward(x, ImagePositions_[p]);
      // need new jacobian ?
      if (p==0 || !ParameterJacobianIsConstant)

        //        if (T_->ParameterJacobianForward(JacParams, x)!=0) return -4;
        if (T_->ParameterJacobianBackward(JacParams, ImagePositions_[p])!=0)
          return -4;


      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
               "propagate parameter uncertainty "<<Cov);
      // approximate isotropic 2d uncertainty of this pixel's position
      BIAS::Matrix2x2<double> PixelCov(JacParams*Cov*JacParams.Transpose());
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL, " to pixel cov "<<PixelCov);
      double scale =
        sqrt(PixelCov.Trace());
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL, " to position uncertainty "<<scale);
      // compute best pyramid level to access and save grey value
      double pyrLevel = 0.0;
      if (scale<=0.5) scale = 0.5;
      else if (scale>I1.size()-1.0) {
        scale = I1.size()-1.0;
        pyrLevel = log2(scale) + 1.0;
      } else {
        pyrLevel = log2(scale) + 1.0;
      }
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL, "Using pyramid level "<<pyrLevel);
      TemplateScales_[p] = scale;
      // check whether we can obtain all grey values:
      int checker = 0;
#ifdef ANISOTROPIC
      checker =
        I1.GetAnisotropicImageValue(x[0], x[1], PixelCov,
                                    TemplateGreyValues_[p]);

      if (checker!=0) {
        // try to obtain reasonable grey value anyway
        if (I1[0]->IsInROI(x[0], x[1]))
          TemplateGreyValues_[p] = I1.GetImageValue(x[0], x[1], 0);
        else {
          BIASERR(x<<" is not in ROI of source!!!");
          TemplateGreyValues_[p] = UNDEF_VALUE;
        }
      }

      // approximate gradient at that level
      // could be improved by simply convolving with gaussian derivative
      double gv1,gv2, gv3,gv4;
      checker+=
        I1.GetAnisotropicImageValue(x[0]+0.5*scale, x[1], PixelCov, gv1);
      checker+=
        I1.GetAnisotropicImageValue(x[0]-0.5*scale, x[1], PixelCov, gv2);
      checker+=
        I1.GetAnisotropicImageValue(x[0], x[1]+0.5*scale, PixelCov, gv3);
      checker+=
        I1.GetAnisotropicImageValue(x[0], x[1]-0.5*scale, PixelCov, gv4);
      Vector2<double> grad(gv1-gv2, gv3-gv4);
#else
      checker =
        I1.GetTrilinearImageValue(x[0], x[1], pyrLevel, TemplateGreyValues_[p]);

      if (checker!=0) {
        // try to obtain reasonable grey value anyway
        if (I1[0]->IsInROI(x[0], x[1]))
          TemplateGreyValues_[p] = I1.GetImageValue(x[0], x[1], 0);
        else {
          BIASERR(x<<" is not in ROI of source!!!");
          TemplateGreyValues_[p] = UNDEF_VALUE;
        }
      }
      double gv1,gv2, gv3,gv4;
      checker+= I1.GetTrilinearImageValue(x[0]+0.5*scale, x[1], pyrLevel, gv1);
      checker+= I1.GetTrilinearImageValue(x[0]-0.5*scale, x[1], pyrLevel, gv2);
      checker+= I1.GetTrilinearImageValue(x[0], x[1]+0.5*scale, pyrLevel, gv3);
      checker+= I1.GetTrilinearImageValue(x[0], x[1]-0.5*scale, pyrLevel, gv4);
      Vector2<double> grad(gv1 - gv2, gv3-gv4);
#endif
      if (p==0 || !TextureJacobianIsConstant)
        if (T_->TextureJacobianForward(x, JacTex)!=0) return -5;

      if (checker!=0) {
        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Could not obtain grey value for "
                 <<x<<" at pyramid level "
                 <<pyrLevel);
        active_[p] = false;
        continue;
      } else {
        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Using "<<x<<" at pyramid level "
                 <<pyrLevel<<" with grey value "<<TemplateGreyValues_[p]);
      }
      // approximate gradient at that level
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Rotate and scale gradient "
               <<grad[0]<<";"<<grad[1]);

      // rotate gradient with texture transformation and divide secant length
      TemplateGradients_[p] = JacTex * (grad * 1.0 / scale);
      active_[p] = TemplateGradients_[p].NormL2()*scale>intensitySigma_;
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"GRAD="<<TemplateGradients_[p]
               <<(active_[p]?" active":" skipped"));


#ifdef BIAS_DEBUG
      if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES)) {
#ifdef ANISOTROPIC
        *pwdata ++ = PixelCov[0][0];
        *pwdata ++ = PixelCov[0][1];
        *pwdata ++ = PixelCov[1][1];
#else
        *pwdata ++ = pyrLevel;
#endif
      }
#endif
      INFNANCHECK(TemplateGreyValues_[p]);
    }
#ifdef BIAS_DEBUG
    // save debug image with weights
    if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES)) {
      //ImageIO::Save(string("tlevels-")+FileHandling::toString(iterationDebug_)
      //               +string(".mip"), *ptheweights);
      ImageIO::Save(string("tlevels-")
                          +FileHandling::toString(iterationDebug_)
                          +string(".mip"), 
                    *ptheweights);
      delete ptheweights;
    }
#endif
  } else {
    // use user specified pyramid level
    const double scale = pow(2.0,PyramidLevel);
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "Using pyramid level "
             <<PyramidLevel<<" for grey values and gradients (scale="<<scale
             <<")");
    Matrix2x2<double> JacTex;
    for (unsigned int p=0; p<TemplatePositions_.size(); p++) {
      const HomgPoint2D& x(TemplatePositions_[p]);
      T_->MapForward(x, ImagePositions_[p]);
      int checker = 0;
      checker += I1.GetTrilinearImageValue(x[0], x[1], PyramidLevel,
                                           TemplateGreyValues_[p]);

      if (checker!=0) {
        // try to obtain reasonable grey value anyway
        if (I1[0]->IsInROI(x[0], x[1]))
          TemplateGreyValues_[p] = I1.GetImageValue(x[0], x[1], 0);
        else {
          BIASERR(x<<" is not in ROI of source!!!");
          TemplateGreyValues_[p] = UNDEF_VALUE;
        }
      }

      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL, "Pixel grey value at "<<x<<" is "
               <<TemplateGreyValues_[p]);
      INFNANCHECK(TemplateGreyValues_[p]);

      // hier kann noch kraeftig optimiert werden, selbe gewichte fuer alle !
      // trilinear ist unnoetig, benutze naechste ganze pyramidenstufe
      // approximate gradient at that level
      double gv1,gv2, gv3,gv4;
      checker += I1.GetTrilinearImageValue(x[0]+0.5*scale, x[1],
                                           double(PyramidLevel), gv1);
      checker += I1.GetTrilinearImageValue(x[0]-0.5*scale, x[1],
                                           double(PyramidLevel), gv2);
      checker += I1.GetTrilinearImageValue(x[0], x[1]+0.5*scale,
                                           double(PyramidLevel), gv3);
      checker += I1.GetTrilinearImageValue(x[0], x[1]-0.5*scale,
                                           double(PyramidLevel), gv4);

      Vector2<double> grad(gv1 - gv2, gv3-gv4);

      // compute jacobian only at first time or when not constant
      if (p==0 || !TextureJacobianIsConstant)
        if (T_->TextureJacobianForward(x, JacTex)!=0) return -6;

      if (checker != 0) {
        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Could not obtain grey value for "
                 <<x<<" at pyramid level "<<PyramidLevel);
        active_[p] = false;
        continue;
      }



      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
               "rotate gradient "<<grad[0]<<";"<<grad[1]
               <<" with local texture warp ["<<JacTex[0][0]<<" "<<JacTex[0][1]
               <<"; "<<JacTex[1][0]<<" "<<JacTex[1][1]<<"]");
      TemplateGradients_[p] = JacTex * (grad * 1.0 / scale);
      double gradnorm =  TemplateGradients_[p].NormL2();
      active_[p] = (gradnorm*scale>intensitySigma_);
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"GRAD="<<TemplateGradients_[p]
               <<(active_[p]?" active":" skipped"));

      if ((BIAS_ISINF(TemplateGreyValues_[p])||BIAS_ISNAN(TemplateGreyValues_[p]))
          &&active_[p]) {
        BIASERR("bad value for active pixel..");
        BIASABORT;
      }
    }
  }

  // -------------  account for appearance / illumination changes ? ---------
  if (AffineBrightnessInvariance_) {
    // use only active ?
    // inactive pixels might not have a correct grey value !
    // active however have a strong gradient which is not suitable for
    // brightness estiamtion!

    // compute mean and scatter of patch
    double mean = 0;
    for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
      mean += TemplateGreyValues_[i];
    }
    mean1_ = mean / TemplateGreyValues_.size();

    double scatter = 0;
    for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
      scatter += (TemplateGreyValues_[i]-mean1_) *
        (TemplateGreyValues_[i]-mean1_);
    }
    scatter = scatter/(TemplateGreyValues_.size()-1);
    if (scatter>0.01*intensitySigma_) dev1_ = sqrt(scatter);
    else dev1_ = 0.1*intensitySigma_;

    // now rescale grey values such that mean=0 and sigma=1 afterwards
    for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
      TemplateGreyValues_[i] = (TemplateGreyValues_[i]-mean1_)/dev1_;
      TemplateGradients_[i] /= dev1_;
    }
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
             "Affine normalization of gradients and grey values yielded mean="
             <<mean1_<<" and dev="<<dev1_);
  } else {
    dev1_  = 1.0;
    mean1_ = 0.0;
  }

  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "compute jacobians + hessian");
  // prepare compositional inverse update (start with 0)
  TextureTransform *pUpdateWarp = T_->Clone();

#ifdef BIAS_DEBUG
  if (GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES) {
    int numberOfPixelsPerRow =
      int(ceil(sqrt(double(TemplateGreyValues_.size()))));
    Image<float> thetemplate(numberOfPixelsPerRow,numberOfPixelsPerRow,1);
    Image<float> thegradients(numberOfPixelsPerRow,numberOfPixelsPerRow,3);
    thegradients.SetZero();
    float *pF = thetemplate.GetImageData();
    float *pG = thegradients.GetImageData();
    for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
      *pF++ = TemplateGreyValues_[i];
      if (active_[i]) {
        // write active into green
        pG++;*pG++ = TemplateGradients_[i].NormL2();pG++;
      } else {
        // write low into into red
        *pG++ = TemplateGradients_[i].NormL2();pG++;pG++;
      }
    }
    /*
    ImageIO::Save(string("template-")+FileHandling::toString(iterationDebug_)
                   +string(".mip"), thetemplate);
    ImageIO::Save(string("gradients-")+FileHandling::toString(iterationDebug_)
                   +string(".mip"), thegradients); */
    
    ImageIO::Save(string("template-")
                        +FileHandling::toString(iterationDebug_)
                        +string(".mip"),
                  thetemplate);
    ImageIO::Save(string("gradients-")
                        +FileHandling::toString(iterationDebug_)
                        +string(".mip"),
                  thegradients);
  }
#endif

  Vector<double> parameterUpdate;
  T_->GetParameters(parameterUpdate);
  for (unsigned int i=0; i<parameterUpdate.Size(); i++)
    parameterUpdate[i] = 0;
  pUpdateWarp->SetParameters(parameterUpdate);

  Matrix<double> Jac;
  Matrix<double> Hessian(parameterUpdate.Size(), parameterUpdate.Size(),
                         MatrixZero);
  Matrix<double> invCovInv;
  unsigned int numMeasurements = 0;
  for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
    // need new jacobian ?
    if (i==0 || !ParameterJacobianIsConstant)
      if (pUpdateWarp->ParameterJacobianBackward(Jac, ImagePositions_[i])!=0)
        return -7;

    // no gradient, skip!
    if (!active_[i]) continue;

    Jac.MultLeft(TemplateGradients_[i], SteepestDescentImages_[i]);
    BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Steepest descent image is "<<
             SteepestDescentImages_[i]);
    // add to hessian"
    Hessian +=
      SteepestDescentImages_[i].OuterProduct(SteepestDescentImages_[i]);
    numMeasurements++;
  }

  // incorporate prior on the warp parameters ?
  if (Dampening_>0.0) {
    // if the intensity noise is 1, then a dampening of 1 leads to the
    // maximum a posteriori estimate of p (without brightness invariance mode)

#ifdef BIAS_DEBUG
    bool CovInvgood = true;
    for (int i=0; i<CovInv.num_rows(); i++) {
      for (int j=0; j<CovInv.num_cols(); j++) {
        if (BIAS_ISNAN(CovInv[i][j]) || BIAS_ISINF(CovInv[i][j])) {
          CovInvgood = false;
        }
      }
    }
    if (!CovInvgood) {
      BIASERR("Bad CovInv "<<CovInv<<" cov was "<<Cov<<" lastPIJ="<<PIJ);
      BIASABORT;
    }
#endif

    SVD covinvsvd(false);
    if (covinvsvd.Compute(CovInv)==0) {
      invCovInv = covinvsvd.Invert();
      Hessian += invCovInv * intensitySigma_ * intensitySigma_ *
        Dampening_ / (dev1_*dev1_);
    } else {
      // cannot decompose
      BIASERR("Bad covariance, cannot decompose: "<<CovInv);
    }
  }

  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,  "Finished Hessian, "
           <<Hessian<<", inverting ...");
  // the hessian must have full rank here, otherwise we have a bad feature:
  // "highdimensional aperture problem"

#ifdef BIAS_DEBUG
  //cout<<"Hessian is "<<setprecision(20)<<Hessian<<endl;
  bool hessiangood = true;
  for (int i=0; i<Hessian.num_rows(); i++) {
    for (int j=0; j<Hessian.num_cols(); j++) {
      if (BIAS_ISNAN(Hessian[i][j]) || BIAS_ISINF(Hessian[i][j])) {
        hessiangood = false;
      }
    }
  }
  if (!hessiangood) {
    BIASERR("Bad Hessian !!!");
    cout<<"Last Jacobian was "<< Jac<<endl;
    for (unsigned int i=0; i<SteepestDescentImages_.size(); i++) {
      cout<<"SDI "<<i<<" ";
      if (active_[i]) {
        cout<<SteepestDescentImages_[i];
      } else {
        cout<<"is disabled."<<endl;
      }
    }
  }
#endif

  SVD Hsvd(Hessian);

  Vector<double> S = Hsvd.GetS();
  if (Dampening_<0.0) {
    for (int i=0; i<Hessian.num_cols(); i++) {
      Hessian[i][i] -= S[0] * Dampening_;
    }
    Hsvd.Compute(Hessian);
  } else if ((S[0]<1e-9) || (S[S.Size()-1]/S[0]<1e-16)) {
    // cannot solve !
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "Not enough structure for "
             <<" uniquely solving for all params in H: "<<Hessian
             <<", singular values are "<<S);
    return -1;
  }
  Hessian = Hsvd.Invert();

  //---------------------       iterate         --------------------
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "Starting iterations");

  int iter = 1;
  bool converged = false;
  Vector<double> errorImage(TemplateGreyValues_.size());
  vector<double> greyValuesI2(TemplateGreyValues_.size());
#ifdef ANISOTROPIC
  vector<Matrix2x2<double> > pyramidLevelsI2(TemplateGreyValues_.size());
#else
  vector<double> pyramidLevelsI2(TemplateGreyValues_.size());
#endif
  Vector<double> steepestDescentImageSum(parameterUpdate.Size());

  // remember the best-so-far residual
  double lastresidual = DBL_MAX;
  double residual = 0;
  double pyrLevel = 0;

  // remember best parameters so far
  Vector<double> lastGoodParameters;
  pUpdateWarp->GetParameters(lastGoodParameters);
  while (!converged) {
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Iteration "<<iter);
    // warp I2 with compositional parameters and compute intensity difference
    HomgPoint2D curPos(0,0);
    steepestDescentImageSum.SetZero();
#ifdef BIAS_DEBUG
    if (GetDebugLevel() & D_IMAGEALIGNMENT_PARAMETER) {
      pUpdateWarp->GetParameters(parameterUpdate);
      BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
               "Current parameter update onto prediction is "
               <<parameterUpdate);
    }
#endif
    // fetch predicted, raw, grey value
    if (PyramidLevel<0) {
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL, "Cov is "<<Cov);
      // use resolution from covariance
      Matrix<double> JacParams;
      BIAS::Matrix<double> covI2;
      for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
        HomgPoint2D &x(ImagePositions_[i]);
        pUpdateWarp->MapBackward(x, curPos);
        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Looking up Pixel at "<< curPos);
        // first iteration: need parameter jacobian fpr pyrLevel calculation:
        if (iter==1) {
          if ((i==0 || !ParameterJacobianIsConstant)) {
            if (pUpdateWarp->ParameterJacobianBackward(JacParams, x)!=0)
              return -8;
            BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
                     " PJBW="<<JacParams);
#ifdef ANISOTROPIC
            covI2 = JacParams*Cov*JacParams.Transpose();
            double scale = covI2.Trace();
#else
            double scale =
              sqrt(BIAS::Matrix<double>
                   (JacParams*Cov*JacParams.Transpose()).Trace());
#endif
            BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
                     " position uncertainty "<<scale);
            // compute best pyramid level to access and save grey value"
            pyrLevel = 0.0;
            if (scale<=0.5) scale = 0.5;
            else if (scale>I1.size()-1.0) {
              scale = I1.size()-1.0;
              pyrLevel = log2(scale) + 1.0;
            } else {
              pyrLevel = log2(scale) + 1.0;
            }
          }
#ifdef ANISOTROPIC
          pyramidLevelsI2[i] = covI2;
#else
          pyramidLevelsI2[i] = pyrLevel;
#endif
#ifdef BIAS_DEBUG
          // save debug image with weights
          if ((GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES) &&
              (i+1==TemplateGreyValues_.size())) {
            int numberOfPixelsPerRow =
              int(ceil(sqrt(double(pyramidLevelsI2.size()))));
#ifdef ANISOTROPIC
            Image<float>
              theweights(numberOfPixelsPerRow,
                         numberOfPixelsPerRow,3);
#else
            Image<float>
              theweights(numberOfPixelsPerRow,
                         numberOfPixelsPerRow,1);
#endif
            theweights.SetZero();
            float *pF = theweights.GetImageData();
            for (unsigned int j=0; j<pyramidLevelsI2.size(); j++) {
#ifdef ANISOTROPIC
              *pF++ = pyramidLevelsI2[j][0][0];
              *pF++ = pyramidLevelsI2[j][0][1];
              *pF++ = pyramidLevelsI2[j][1][1];
#else
              *pF++ = pyramidLevelsI2[j];
#endif
            }
            //ImageIO::Save(string("levels-")+FileHandling::toString(iterationDebug_)
            //               +string(".mip"), theweights);
            ImageIO::Save(string("levels-")
                                +FileHandling::toString(iterationDebug_)
                                +string(".mip"),
                          theweights);
          }
#endif
        }

#ifdef ANISOTROPIC
        if (I2.GetAnisotropicImageValue(curPos[0],curPos[1],pyramidLevelsI2[i],
                                        greyValuesI2[i])!=0) {
          // remove from active set!
          greyValuesI2[i] = TemplateGreyValues_[i];
          active_[i] = false;
        }

#else
        if (I2.GetTrilinearImageValue(curPos[0], curPos[1], pyramidLevelsI2[i],
                                      greyValuesI2[i])!=0) {
          // remove from active set!
          greyValuesI2[i] = TemplateGreyValues_[i];
          active_[i] = false;
        }
#endif

        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Using "<<curPos
                 <<" at pyramid level "<<pyramidLevelsI2[i]
                 <<" with grey value(2) "<<greyValuesI2[i]);
        INFNANCHECK(greyValuesI2[i]);
      }
    } else {
      // user specified pyramid level
      for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
        pUpdateWarp->MapBackward(ImagePositions_[i], curPos);
        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Looking up Pixel at "<< curPos);
        if (I2.IsInROI(curPos[0], curPos[1], PyramidLevel))
          greyValuesI2[i] =
            I2.GetImageValue(curPos[0], curPos[1], PyramidLevel);
        else {
          BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Not in ROI, disabling! ");
          greyValuesI2[i] = TemplateGreyValues_[i];
          active_[i] = false;
        }
        BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Grey value is "<< greyValuesI2[i]);
        INFNANCHECK(greyValuesI2[i]);
      }
    }

    if (AffineBrightnessInvariance_) {

      // check active here ?!

      // compute mean+scatter of patch
      double mean = 0;
      for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
        mean += greyValuesI2[i];
      }
      mean2_ = mean / TemplateGreyValues_.size();
      double scatter = 0;
      for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
        scatter += (greyValuesI2[i] - mean2_)*(greyValuesI2[i] - mean2_);
      }
      scatter /= TemplateGreyValues_.size()-1;
      if (scatter>0.01*intensitySigma_) dev2_ = sqrt(scatter);
      else dev2_ = 0.1*intensitySigma_;

      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
               "Affine normalization of grey values(2) yielded mean="
               <<mean2_<<" and dev="<<dev2_);

      // compute normalized grey value residuum
      for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
        greyValuesI2[i] = (greyValuesI2[i]-mean2_)/dev2_;
      }
    }
#ifdef BIAS_DEBUG
    if (GetDebugLevel() & D_IMAGEALIGNMENT_IMAGES) {
      int numberOfPixelsPerRow = int(ceil(sqrt(double(greyValuesI2.size()))));
      Image<float> theimage(numberOfPixelsPerRow,numberOfPixelsPerRow,1);
      float *pF = theimage.GetImageData();
      for (unsigned int g=0; g<greyValuesI2.size(); g++) {
        *pF++ = greyValuesI2[g];
      }
      /*
      ImageIO::Save(string("image-")+FileHandling::toString(iterationDebug_)
                     +string("-")+FileHandling::toString(iter)+string(".mip"),
                     theimage); */
      ImageIO::Save(string("image-")
                          +FileHandling::toString(iterationDebug_)
                          +string("-")
                          +FileHandling::toString(iter)
                          +string(".mip"),
                    theimage);
    }
#endif

    //Vector<double> b(TemplateGreyValues_.size());
    //Matrix<double> A(TemplateGreyValues_.size(), 2, MatrixZero);

    // now weight steepestDescent images by residuum
    residual = 0;
    for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
      // no good gradient ...
      if (!active_[i]) {
        //        A[i][0] = 0;
        //        A[i][1] = 0;
        //        b[i] = 0;
        continue;
      }
      //      A[i][0] = dev2_ * greyValuesI2[i] + mean2_;
      //      A[i][1] = 1;
      //      b[i] = dev1_ * TemplateGreyValues_[i] + mean1_;

      errorImage[i] = greyValuesI2[i] - TemplateGreyValues_[i];
      residual += errorImage[i]*errorImage[i];
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,"Grey value difference is "<<
               errorImage[i]);

      steepestDescentImageSum += errorImage[i] * SteepestDescentImages_[i];

    }
    //    SVD mysvd(A.Transpose()*A);
    //    Vector<double> result = mysvd.Invert()*A.Transpose()*b;
    //    cout<<" brightness is "<<result<<endl;

    residual = sqrt(residual/numMeasurements);
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "RMS residual is "<<residual);
    if (residual>lastresidual) {
      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "Error increase from  "
               <<lastresidual<<". Stopping.");
      if (LineSearch_>0.0) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "Step did not improve, trying a "
                 <<LineSearch_<<" smaller step.");
        pUpdateWarp->SetParameters(lastGoodParameters);
        // reduce update
        parameterUpdate *= LineSearch_;
      } else {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS, "No further improvement.Stopping.");
        // error did not improve ...
        pUpdateWarp->SetParameters(lastGoodParameters);
        status = 2;
        break;
      }
    } else {
      pUpdateWarp->GetParameters(lastGoodParameters);
      lastresidual = residual;
      // solve for parameter update
      if (Dampening_<=0.0) {
        Hessian.Mult(steepestDescentImageSum, parameterUpdate);
      } else {
        // For MAP estimation with a prior centered on the original parameters
        // its original mean must be considered here. Otherwise the prior
        // moves with iterations from the current parameter set.

        // obtain weighting of "correct" prior with scaled intensities
        double weightfactor = (intensitySigma_*intensitySigma_) / (dev2_*dev2_);
        parameterUpdate = Hessian * (steepestDescentImageSum -
             weightfactor*invCovInv*pUpdateWarp->GetInverseParameters());

#ifdef BIAS_DEBUG
        for (unsigned int i=0; i<parameterUpdate.Size(); i++) {
          if (BIAS_ISINF(parameterUpdate[i]) || BIAS_ISNAN(parameterUpdate[i])) {
            BIASERR("bad result:"<<parameterUpdate<<" invCovinv="<<invCovInv
                    <<" puw="<<pUpdateWarp->GetInverseParameters()
                    <<" hessian="<<Hessian<<" sdis="<<steepestDescentImageSum);



            cout<<"Grey value data ("<<TemplateGreyValues_.size()<<")"<<endl;
            for (unsigned int r=0; r<TemplateGreyValues_.size(); r++) {
              // no good gradient ...
              if (!active_[r]) continue;
              cout<< errorImage[r] <<"="
                  <<greyValuesI2[r]<<"-"<< TemplateGreyValues_[r];

              cout<<"SDI="<< SteepestDescentImages_[i]<<endl;
            }
            BIASABORT;
          }
        }
#endif
      }
    }
    BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,
             "inv.compositional parameter update is "
             <<parameterUpdate);
    if (parameterUpdate.NormL2()>1000) {
      // we expect a change close to the identity => p=0
      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Diverged! ");
      return -20;
    }
    // concatenate parameters
    pUpdateWarp->ComposeWithInverseDeltaP(parameterUpdate);
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Update of "
             <<parameterUpdate.NormL2()<<" thresh="<<MaxUpdate_);
    if (parameterUpdate.NormL2()<MaxUpdate_) {
      converged = true;
      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Convergence ! Update = "
               <<parameterUpdate.NormL2());
    } else if (++iter>MaxIterations_) {
      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Too many iterations: "<<iter);
      status = 1;
      break;
    }
  }
  pUpdateWarp->GetParameters(parameterUpdate);
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
           "Finished. Total parameter update onto prediction is "
           <<parameterUpdate);
  // merge both direction parameters
  T_->ComposeWithInverseDeltaP(parameterUpdate);
  // and output parameters
  T_->GetParameters(params);
#ifdef BIAS_DEBUG
  for (unsigned int i=0; i<params.Size(); i++) {
    INFNANCHECK(params[i]);
  }
#endif
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Final warp parameters are "<<params);

  // update covariance
  if (numMeasurements>params.Size()) {
    BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,"residual is now "<<residual);
    double additionalresidual = 0;
    for (unsigned int i=0; i<TemplateGreyValues_.size(); i++) {
      // no good gradient ...
      if (!active_[i]) continue;
      // if we used this pixel not at the highest resolution, we must expect
      // the grey value residual to be larger in reality
      if (TemplateScales_[i]>0.5)
        additionalresidual +=
          (TemplateGradients_[i]*(TemplateScales_[i]-0.5)).NormL2();
    }
    residual += additionalresidual/double(numMeasurements);
    BIASDOUT(D_IMAGEALIGNMENT_PARAMETER,"residual changed to "<<residual);


    // avoid singular ref variance by adding 1% of intensity sigma
    double refvar = 0.0001 +
      (residual*residual*dev1_*dev1_) * double(numMeasurements) /
      (double(numMeasurements-params.size())*intensitySigma_*intensitySigma_);


    // assume parameterUpdate is nearly zero
    Matrix<double> upJac;
    if (pUpdateWarp->ParameterInversionJacobian(upJac)!=0) return -10;

#ifdef BIAS_DEBUG
    //    cout<<"Hessian is "<<setprecision(20)<<Hessian<<endl;
    bool hessiangood = true;
    for (int i=0; i<upJac.num_rows(); i++) {
      for (int j=0; j<upJac.num_cols(); j++) {
        if (BIAS_ISNAN(upJac[i][j]) || BIAS_ISINF(upJac[i][j])) {
          hessiangood = false;
        }
      }
    }
    if (!hessiangood) {
      Vector<double> badparams(6);
      pUpdateWarp->GetParameters(badparams);
      BIASERR("Bad upjac !!!"<< upJac<<" or PIJ "<<PIJ<<" for params "
              <<badparams);
    }
#endif

    // combine jacobians
    SVD covtransformSVD(upJac*PIJ);
    PIJ = covtransformSVD.Invert();
    Hessian *= refvar;
    Cov = PIJ * Hessian * PIJ.Transpose();
  } else {
    // no redundancy, perfect linear fit ...
    Cov.SetIdentity();
    Cov *= 1e20;
    BIASWARN("Could not estimate covariance, setting large value");
  }

  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Covariance changed to "<<Cov);
  return status;
}


void BIAS::ImageAlignment::
SetAlignmentPixelsRectangular(const LocalAffineFrame& LAF,
                              int numberOfPixelsPerRow)
{
  // automatic resolution ?
  if (numberOfPixelsPerRow<1) {
    // use feature area ("all" pixels at full resolution
    numberOfPixelsPerRow = int(rint(sqrt(4.0*LAF.GetA().det())))+1;
    BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"NumOfPixelsPerRow automatically "
             <<" computed as "<<numberOfPixelsPerRow<<" with A="<<LAF.GetA());
  }

  double pixeldistance = 2.0/(numberOfPixelsPerRow-1);
  if (numberOfPixelsPerRow<2) pixeldistance = 1.0;

  int numberOfPixels = numberOfPixelsPerRow * numberOfPixelsPerRow;
  TemplateGreyValues_.resize(numberOfPixels);
  TemplateGradients_.resize(numberOfPixels);
  TemplatePositions_.resize(numberOfPixels);
  TemplateScales_.resize(numberOfPixels);
  ImagePositions_.resize(numberOfPixels);
  SteepestDescentImages_.resize(numberOfPixels);
  active_.resize(numberOfPixels);
  ControlPositions_.resize(4);
  const double offset = -1.0;
  for (int y=0; y<numberOfPixelsPerRow; y++) {
    for (int x=0; x<numberOfPixelsPerRow; x++) {
      int index = y*numberOfPixelsPerRow+x;
      TemplatePositions_[index] =
        LAF.LocalToGlobal(HomgPoint2D(x*pixeldistance+offset,
                                      y*pixeldistance+offset));
      BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
               "Using Position "<<TemplatePositions_[y*numberOfPixelsPerRow+x]);
    }
  }
  ControlPositions_[0] = TemplatePositions_[0];
  ControlPositions_[1] = TemplatePositions_[numberOfPixelsPerRow-1];
  ControlPositions_[2] =
    TemplatePositions_[numberOfPixels-numberOfPixelsPerRow-1];
  ControlPositions_[3] =
    TemplatePositions_[numberOfPixels-1];

  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Have "<<numberOfPixels
           <<" pixels with distance "<<pixeldistance<<" in local affine frame");

}




int BIAS::ImageAlignment::AutoAlign(const PyramidImage<float>& I1,
                                    const PyramidImage<float>& I2,
                                    Vector<double>& params,
                                    Matrix<double>& Cov) {
  int returnvalue = 0;
  double oldLineSearch = LineSearch_;

  double trace =  Cov.Trace();
  double lasttrace = trace + 1e100;
  Matrix<double> initialCov(Cov);
  double desiredAccuracy = GetMaxUpdate();
  const unsigned int numparams = params.size();
  BIASASSERT(Cov.num_rows()==int(numparams));
  BIASASSERT(Cov.num_cols()==int(numparams));
  iterationDebug_ = 0;
  Matrix<double> lastCov(4.0*Cov);
  try {
    while (trace<lasttrace) {
      lasttrace = trace*0.95; // want 5% improvement to continue

      // make symmetric and slightly decrease
      for (unsigned int i=0; i<numparams; i++) {
        for (unsigned int j=i; j<numparams; j++) {
          if (BIAS_ISINF(Cov[j][i]) || BIAS_ISNAN(Cov[j][i])) {
            BIASERR("corrupted cov, giving up "<<Cov);
            return -1;
          }
          Cov[j][i] = Cov[i][j] =
            (Cov[i][j] + Cov[j][i] + 0.5*lastCov[j][i])*0.25;
        }
        // if any diagonal element got more uncertain, set row/col back
        if (Cov[i][i]>lastCov[i][i]) {
          BIASDOUT(D_IMAGEALIGNMENT_PARAMETER, "Cov increased:"<<Cov
                   <<" from "<<lastCov);
          if (lastCov[i][i]<1e-10) lastCov[i][i] = 1e-10;
          double factor = sqrt(lastCov[i][i]/Cov[i][i]);
          for (unsigned int j=0; j<numparams; j++) {
            Cov[i][j] = Cov[j][i] = Cov[j][i]*factor;
          }
          Cov[i][i] *= factor;
          BIASDOUT(D_IMAGEALIGNMENT_PARAMETER, "Cov clipped to "<<Cov);
        }
        Cov[i][i] += 1e-10; // avoid zero digonal
      }
      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Aligning with Cov="<<Cov);
      // we expect uncertainty in the range of sqrt(trace), if update is
      // much smaller (e.g. only 10%) declare convergence
      double newthresh = 0.03*sqrt(trace);
      if (newthresh<desiredAccuracy) newthresh = desiredAccuracy;
      SetUpdateThreshold(newthresh);
      Vector<double> lastP = params;
      lastCov = Cov;





      // now call align with new resolution
      returnvalue = Align(I1, I2, params, Cov, -1);


      //cout<<"One alignment iteration finshed... press enter!"<<endl;
      //getchar();



      if (returnvalue<0) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Align returned "<<returnvalue
                 <<". Stopping iteration.");
        throw int(1);
      }
      if (iterationDebug_++ > MaxIterations_) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Too many iterations: "
                 <<iterationDebug_);
        returnvalue = 1;
        throw int(2);
      }
      trace = Cov.Trace();


      // finish not before best layer is used for each pixel !
      // use "control pixels"

      /*
        lastP -= params;
        if (lastP.NormL2()<desiredAccuracy) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Covariance reached desired accuracy");
        break;
        }
      */
      if (CheckConvergence_(params, Cov, 0.1)==0) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
                 "Covariance reached desired accuracy: "<<Cov);
        break;
      }
    }
  }
  catch (...) {
    SetUpdateThreshold(desiredAccuracy);
    iterationDebug_ = 0;
    LineSearch_ = oldLineSearch;
    Cov = initialCov;
    return returnvalue;
  }
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Covariance trace="<<trace
           <<" while last time it was "<<lasttrace
           <<", now using full resolution");

  SetUpdateThreshold(desiredAccuracy);
  LineSearch_ = oldLineSearch;
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Aligning with full resolution.");
  returnvalue = Align(I1, I2, params, Cov, 0);
  iterationDebug_ = 0;
  return returnvalue;
}





int BIAS::ImageAlignment::StrictPyramidAlign(const PyramidImage<float>& I1,
                                             const PyramidImage<float>& I2,
                                             Vector<double>& params,
                                             Matrix<double>& Cov) {
  BIASASSERT(I1.size()>0);
  BIASASSERT(I1.size()==I2.size());

  int returnvalue = 0;
  double oldLineSearch = LineSearch_;
#ifdef BIAS_DEBUG
  double lasttrace = 1e200;
  double trace = 1e100;
#endif


  double desiredAccuracy = GetMaxUpdate();
#ifdef BIAS_DEBUG
  const unsigned int numparams = params.size();
#endif
  BIASASSERT(Cov.num_rows()==int(numparams));
  BIASASSERT(Cov.num_cols()==int(numparams));
  iterationDebug_ = 0;
  Matrix<double> lastCov(Cov), initialCov(Cov);
  try {
    for (int pyrLevel = int(I1.size()-1); pyrLevel>0; pyrLevel--) {

      BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Aligning on level "<<pyrLevel);
#ifdef BIAS_DEBUG
      for (unsigned int i=0; i<numparams; i++) {
        INFNANCHECK(params[i]);
      }
#endif
      // now call align with new resolution
      Vector<double> oldparams(params);
      Cov = initialCov;// reset prior
      returnvalue = Align(I1, I2, params, Cov, pyrLevel);
      if (returnvalue<0) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Align returned "<<returnvalue
                 <<". Trying smaller pyramid level.");
        continue;
      } else {
        //cout<<pyrLevel<<": params optimized from "<<oldparams<<" to "<<params<<endl;
      }
      if (iterationDebug_++ > MaxIterations_) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Too many iterations: "
                 <<iterationDebug_);
        returnvalue = 1;
        throw int(2);
      }
#ifdef BIAS_DEBUG
      trace = Cov.Trace();
#endif

      if (CheckConvergence_(params, Cov, 0.1)==0) {
        BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
                 "Covariance reached desired accuracy");
        break;
      }
    }
  }
  catch (...) {
    SetUpdateThreshold(desiredAccuracy);
    iterationDebug_ = 0;
    LineSearch_ = oldLineSearch;
    return returnvalue;
  }
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Covariance did not improve, trace="<<trace
           <<" while last time it was "<<lasttrace);

  SetUpdateThreshold(desiredAccuracy);
  LineSearch_ = oldLineSearch;
  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,"Aligning with full resolution.");
  returnvalue = Align(I1, I2, params, Cov, 0);
  iterationDebug_ = 0;
  return returnvalue;
}



int BIAS::ImageAlignment::
CheckConvergence_(const Vector<double>& params, const Matrix<double>& Cov,
                  const double& pixelAccuracy)
{
  // make sure the user has set a warp model
  BIASASSERT(T_!=NULL);

  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
           "compute template and gradients from warped I1 with prediction "
           <<params);
  T_->SetParameters(params);

  // check whether we have to re-evaluate for each pixel:
  const bool ParameterJacobianIsConstant = T_->ParameterJacobianIsConstant();

  BIASDOUT(D_IMAGEALIGNMENT_PROGRESS,
           "auto pyramid, determine required scale for each pixel");
  Matrix<double> JacParams;
  for (unsigned int p=0; p<ControlPositions_.size(); p++) {
    // which pixel in the source image ?
    const HomgPoint2D& x(ControlPositions_[p]);
    // need new jacobian ?
    if (p==0 || !ParameterJacobianIsConstant)
      T_->ParameterJacobianForward(JacParams, x);

    BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL,
             "propagate parameter uncertainty "<<Cov);
    // approximate isotropic 2d uncertainty of this pixel's position
    double scale =
      sqrt(BIAS::Matrix<double>(JacParams*Cov*JacParams.Transpose()).Trace());
    BIASDOUT(D_IMAGEALIGNMENT_PERPIXEL, " to position uncertainty "<<scale);

    if (scale>pixelAccuracy) return 1;
  }
  return 0;
}