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


/**
   @example EvaluateAlignment.cpp
   @relates ImageAlignment, TextureTransformHomography,TextureTransformAffine
   @brief Alignment evaluation
   @ingroup g_examples
   @author woelk
*/

#include <Base/Image/Image.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Geometry/HomgPoint2D.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Image/TextureTransformHomography.hh>
#include <Image/TextureTransformAffine.hh>
#include <Base/Math/Random.hh>
#include "MathAlgo/Lapack.hh"
#include <Matcher2D/ImageAlignment.hh>
#include <Image/HomographyMapping.hh>
#include <Geometry/RMatrix.hh>
#include <iostream>
#include <iomanip>
#include "Base/Common/FileHandling.hh"
#include "Utils/ThreeDOut.hh"
using namespace BIAS;
using namespace std;


// loads an image and converts to single channel float image
int LoadImage(const string& name, Image<float>& im)
{
  ImageBase baseim;

  // load image
  if (ImageIO::Load(name, baseim)!=0){
    BIASERR("error loading image "<<name);
    return -1;
  }
  // convert to float
  if (ImageConvert::ConvertST(baseim, im, ImageBase::ST_float)!=0){
    BIASERR("error converting image "<<name);
    return -2;
  }
  // convert to grey if necessary
  if (im.GetChannelCount()!=1){
    if (ImageConvert::ToGrey(im, im)!=0){
      BIASERR("error converting image to grey "<<name);
      return -3;
    }
  }
  im.SetMetaData(*baseim.GetMetaData());

  return 0;
}


// the main function
int main(int argc, char*argv[])
{
  // read parameter from command line
  if (argc<2) {
    cerr << argv[0] << " <image> "<< endl;
    return -1;
  }
  // load first image
  Image<float> im[2];
  if (LoadImage(argv[1], im[0]) != 0) {
    BIASERR("error loading image " << argv[0]);
    return -1;
  }
  im[1] = im[0];
  //  Gauss<float, float> smoother;
  // smoother.SetSigma(2.0);
  //smoother.Filter(im[1], im[0]);
  im[1].SetZero();

  Random R;
  Vector<double> par, origpar;
  Matrix<double> cov;
  TextureTransformAffine *TT = new TextureTransformAffine;
  cout<<"Testing AFFINITY"<<endl;
  par.newsize(6); par[0] = 0.0; par[1] = 0; par[2] = 0; par[3] = 0;
  par[4] = 0; par[5] = 0;

  cov.newsize(par.Size(), par.Size());
  cov.SetIdentity();

  cout<<"using parameters "<<par<<endl;
  origpar = par;
  TT->SetParameters(par);

  Vector2<double> thepoint((im[0].GetWidth()-1.0)/2.0,
                           (im[0].GetHeight()-1.0)/2.0);




  cout<<"setting origin !"<<thepoint<<endl;
  TT->SetOrigin(thepoint);

  // get the point to track
  HomgPoint2D p[2];
  p[0][2] = p[1][2] = 1.0;
  p[0][0] = im[0].GetWidth()/2.0;
  p[0][1] = im[0].GetHeight()/2.0;

  LocalAffineFrame LAF;
  LAF.SetT(thepoint);
  double d=20;// halfwinsize
  LAF.SetA(Matrix<double>(2,2,MatrixIdentity)*d);


  ImageAlignment IA;
  IA.SetTextureTransform(*TT);
  //IA.AddDebugLevel(D_IMAGEALIGNMENT_PROGRESS);
  //IA.AddDebugLevel(D_IMAGEALIGNMENT_PARAMETER);
  //IA.AddDebugLevel(D_IMAGEALIGNMENT_PERPIXEL);
  IA.AddDebugLevel(D_IMAGEALIGNMENT_IMAGES);

  IA.SetMaxIterations(50);
  IA.SetUpdateThreshold(0.001);
  IA.SetDampening(0.0001);



  KMatrix K(MatrixIdentity);
  K[1][1] =K[0][0] = im[0].GetWidth()*2.0;//2
   //K[1][1] = im[0].GetHeight()/2.0;
  K[0][2] = (im[0].GetWidth()-1.0)/2.0;
  K[1][2] = (im[0].GetHeight()-1.0)/2.0;
  cout << "K is "<<K<<endl;
  KMatrix Kinv;
  K.GetInverse(Kinv);
  //ImageIO::Save("im0.mip", im[0]);
  ImageIO::Save("im0.mip", im[0]);

  Matrix<double> PredictedCov(6,6,MatrixIdentity);
  for (unsigned int i=0;i<4;i++) PredictedCov[i][i] = 0.01;
  Matrix<double> EmpiricCov(6,6,MatrixIdentity);
  for (unsigned int i=0; i<4; i++)
    EmpiricCov[i][i] = 0.5/(d*d*2.0);
  for (unsigned int i=4; i<6; i++)
    EmpiricCov[i][i] = 0.5;


  ofstream logfile("trackingerror.log");
  logfile<<"# log written by "<<argv[0]<<" hws="<<d<<endl<<flush;
  logfile<<"# angle empiricdistance preddistance distance cornercovdistance result"<<endl<<flush;
  PMatrix P1(MatrixIdentity);
  P1 = K * P1;
  P1.InvalidateDecomposition();
  ThreeDOut vrml;
  vrml.AddPMatrix(P1,im[0].GetWidth(), im[0].GetHeight(), RGBAuc(0,255,0,255),0.1);
  vrml.AddPoint(Vector3<double>(1,0,1), RGBAuc(255,0,0,255));
  vrml.AddPoint(Vector3<double>(0,0,1), RGBAuc(255,255,0,255));
  for (unsigned int angle=0; angle<80; angle+=2) {
    cout<<"angle is now "<<angle<<" degree."<<endl;
    // prepare alignment
    PyramidImage<float> i1;

    i1.Init(im[0]);
    im[1].SetZero();







    RMatrix Rot;
    Vector3<double> axis(0,1,0);
    Rot.Set(axis,double(angle)/180.0*M_PI);
    cout<<"Rot is "<<Rot<<endl;
    Vector3<double> C(0,0,-1);
    C = Rot * C - C;
    cout <<"C is "<<C<<endl;
    Vector3<double> normal(0,0,1);
    Vector3<double> s(0,0,1);
    PMatrix P2(K,Rot,C);
    vrml.AddPMatrix(P2,im[0].GetWidth(), im[0].GetHeight(), RGBAuc(0,0,255,255),0.1);
    HMatrix H;
    H.SetIdentity();
    H.MapAcrossPlane(P2, P1, HomgPlane3D(0,0,1,-1));
    /*
    H = s.ScalarProduct(normal) * Rot  +
      C.OuterProduct(normal);

    H = K * H * Kinv;
    */
    HomographyMapping<float, float> HM;
    HM.SetHomography(H);
    HM.Map(im[0], im[1], MapAnisotropic, false);


    // some noise on image 2
    double pixelnoise = 0.0;
    if (pixelnoise>0.0) {
      float *pData = im[1].GetImageData();
      const float *pDataEnd = im[1].GetImageData()+im[1].GetPixelCount();
      do {
        *pData = R.GetNormalDistributed(*pData, pixelnoise);
      } while (pData++ < pDataEnd);
    }
    stringstream ss;
    ss<<"im1-"<<FileHandling::toString(angle)<<".mip";

    //ImageIO::Save(ss.str(), im[1]);
    ImageIO::Save(ss.str(), im[1]);

    PyramidImage<float> i2;
    i2.Init(im[1]);

    cout<<"Setting alignment pixels "<<endl<<flush;
    IA.SetAlignmentPixelsRectangular(LAF,2*d+1);
    cout<<"Aligning ..."<<endl<<flush;


    HMatrix HLin;
    H.GetLinearized(HomgPoint2D(thepoint)).GetInverse(HLin);

    //cout<<"Linearized is "<<HLin<<" again: "<<endl
    //    <<HLin.GetLinearized(HomgPoint2D(thepoint))<<endl;

    Vector<double> gtpar(6);
    par = gtpar = TT->ExtractParameters(HLin);


    // align images
    cov = EmpiricCov;
    int  result = IA.AutoAlign(i1, i2, par, cov);
    //cout<<"result is "<<par<<endl;

    if (result!=0) {
      BIASERR("alignment failed !!!"<<result);
    }
    cout<<"angle is "<<angle<<endl;
    if (angle%10==0) getchar();
    //int  result = //IA.AutoAlign(i1, i2, par, cov);
    //  IA.StrictPyramidAlign(i1, i2, par, cov);

    vector<BIAS::Vector<double> > cornerParams(5, Vector<double>(6));
    double linearizationerror = 0;
    for (unsigned int corner=0; corner<5; corner++) {
      HomgPoint2D cornerpoint;
      switch (corner) {
      case 0: cornerpoint = HomgPoint2D(thepoint[0]-d,thepoint[1]-d); break;
      case 1: cornerpoint = HomgPoint2D(thepoint[0]-d,thepoint[1]+d); break;
      case 2: cornerpoint = HomgPoint2D(thepoint[0]+d,thepoint[1]-d); break;
      case 3: cornerpoint = HomgPoint2D(thepoint[0]+d,thepoint[1]+d); break;
      default:;
      }
      linearizationerror +=
        H.ComputeLinearizationError(HomgPoint2D(thepoint),cornerpoint);
      HMatrix HLinTmp;
      H.GetLinearized(cornerpoint).GetInverse(HLinTmp);
      cornerParams[corner] = TT->ExtractParameters(HLinTmp);
      //cout<<"affine transform at corner is "<<cornerParams[corner]<<endl;
    }

    BIAS::Vector<double> meanCornerParam(6,0.0);
    for (unsigned int i=0; i<cornerParams.size(); i++) {
      meanCornerParam += cornerParams[i];
    }
    meanCornerParam *= 1.0/double(cornerParams.size());
    Matrix<double> cornerCov(6,6,MatrixZero);
    for (unsigned int i=0; i<cornerParams.size(); i++) {
      cornerCov +=
        BIAS::Vector<double>(meanCornerParam - cornerParams[i]).OuterProduct( BIAS::Vector<double>(meanCornerParam - cornerParams[i]));
    }
    cornerCov *= 1.0/double(cornerParams.size()-1.0);
    //cout<<"cornercov is "<<cornerCov<<endl;
    for (unsigned int i=0; i<6; i++) cornerCov[i][i] += 1e-8;

    linearizationerror /= 4.0;
    cout<<"average linearization error is "<<linearizationerror<<endl;
    PredictedCov.SetIdentity();
    PredictedCov *= 1e-8;
    for (unsigned int i=0; i<4; i++)
      PredictedCov[i][i] += linearizationerror*linearizationerror/(d*d*2.0);
    for (unsigned int i=4; i<6; i++)
      PredictedCov[i][i] += linearizationerror*linearizationerror;

    gtpar -= par;


    double empiricdistance = MahalanobisDistance(EmpiricCov,gtpar);
    cout<<"empiric distance is "<<empiricdistance<<endl;
    double preddistance = MahalanobisDistance(PredictedCov,gtpar);
    cout<<"compensated distance is "<<preddistance<<endl;
    for (unsigned int i=0; i<6; i++) cov[i][i] += 1e-8;
    double distance = MahalanobisDistance(cov,gtpar);
    cout<<"obtained data distance is "<<distance<<endl;
    double cornercovdistance = MahalanobisDistance(cornerCov,gtpar);
    cout<<"corner cov distance is "<<cornercovdistance<<endl;
    if (result>=0) {
      logfile<<angle<<" "<<empiricdistance<<" "<<preddistance<<" "<<distance<<" "<<cornercovdistance<<" "<<result<<endl<<flush;

    }

  }
  vrml.VRMLOut("scene.wrl");
  return 0;
}