ExampleTrackerBase.cpp

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

/** @example ExampleTrackerBase.cpp
    @relates TrackerBaseSimple
    @brief This little example evaluates a TrackerBase class against synthetically
    generated ground truth data.
    Synthetically generated ground truth data can be found in the TestData
    repository in our cvs.

    ExampleTrackerBase 157 157 ~/cvs/TestData/tracker_noise/image*.pgm

    @ingroup g_examples
    @author woelk 
*/


#include <Matcher2D/TrackerBaseSimple.hh>
#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 <Filter/GradientSobel3x3.hh>
#include <Filter/GradientGauss.hh>
#include <Filter/GradientGaussAsymmetric.hh>
#include <Filter/Binomial.hh>

#include <iostream>
#include <iomanip>

using namespace BIAS;
using namespace std;


// always track using float image, since it is a lot faster
#define StorageType float

// default parameter file name
#define DEFAULT_PARA_FILE_NAME "TrackerBase.ini"


/// loads an image and converts to single channel float image
int LoadImage(char *name, Image<StorageType>& 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());

  //ImageIO::Save("fimage.mip", im);
  return 0;
}

/// draws the tracking result and saves to name.tracked
void Draw(Image<StorageType>& im, HomgPoint2D& p, Matrix2x2<KLT_TYPE>& cov,
          char *name)
{
  Image<unsigned char> dim;
  unsigned uip[2], radius=1;
  unsigned char color[]={255,0,0};
  string file;
  float cov_scale=1e5;
  bool draw_cov=true;


  // convert to unsigned char
  if (ImageConvert::ConvertST(im, dim, ImageBase::ST_unsignedchar)!=0){
    BIASERR("error converting image to unsigned char "<<name);
    BIASABORT;
  }

  // convert to rgb if necessary
  if (dim.GetColorModel()!=ImageBase::CM_RGB){
    if (ImageConvert::ToRGB(dim, dim)!=0){
      BIASERR("error converting image to RGB "<<name);
      BIASABORT;
    }
  }

  uip[0]=(unsigned int)rint(p[0]);
  uip[1]=(unsigned int)rint(p[1]);

  ImageDraw<unsigned char>::
    CircleCenterFilled(dim, uip[0], uip[1], radius, color);

  if (draw_cov){
    double center[2], a[2], b[2], eva, evb;
    Vector2<double> na, nb;
    center[0]=p[0];
    center[1]=p[1];

    cov.EigenvalueDecomposition(eva, na, evb, nb);
    cout << eva<<", "<<evb<<"\n";
    a[0]=na[0]*eva*cov_scale;
    a[1]=na[1]*eva*cov_scale;
    b[0]=nb[0]*evb*cov_scale;
    b[1]=nb[1]*evb*cov_scale;
    ImageDraw<unsigned char>::
      Ellipse(dim, center, a, b, color);
  }

  file=name;
  file+=".tracked";
  //ImageIO::Save(file, dim);
  ImageIO::Save(file, dim);
  cout << "wrote "<<file<<endl;
}

/// the main function
int main(int argc, char*argv[])
{
  Param para;
  para.SetWriteOptions(false, false, false);
  Image<StorageType> fim[2], im[2], drawim;
  Image<StorageType> gradx[2], grady[2];
  KLT_TYPE error, err, residuumMAD, residuumMSD, errorsum=0.0;
  HomgPoint2D p[2];
  Matrix2x2<KLT_TYPE> cov;
  int newim, oldim, iter;
  int res, counter;
  bool debug=false;
  bool draw=true;
  MetaData *md;
  AppData appdata;
  HomgPoint2D realpoint;

  TrackerBaseInterface<StorageType> *tracker =
    new TrackerBaseSimple<StorageType>(para);
  FilterNTo2N<StorageType, StorageType> *grad=NULL;
  GradientSobel3x3<StorageType, StorageType> gsobel3x3(para);
  GradientGauss<StorageType,StorageType> ggauss(para);
  GradientGaussAsymmetric<StorageType, StorageType>
    ggaussasymmetric(para);
  int *gradienttype = AddGradientType(para);
  Binomial<StorageType, StorageType> binomial(para);

  TimeMeasure timer, tc;

  int argind = para.UpdateParameter(argc, argv, DEFAULT_PARA_FILE_NAME);

  if (argc-argind<4 || argind<1){
    cerr << argv[0] << " : <x-coo> <y-coo> <image> <image> ..."<< endl;
    return -5;
  }

  // activate some debugging output of tracking object
  //tracker.AddDebugLevel(D_TRACKERB_INIT);
  //tracker.AddDebugLevel(D_TRACKERB_SKLT);
  //tracker.AddDebugLevel(D_TRACKERB_GKLT);
  //tracker.AddDebugLevel(D_TRACKERB_BILINEAR);

  // choose the gradient type according to parameter
  switch (*gradienttype){
  case GT_Sobel3x3:
    grad = &gsobel3x3;
    cerr <<"Sobel3x3\n";
    break;
  case GT_Gauss:
    grad = &ggauss;
    cerr <<"GradientGauss\n";
    break;
  case GT_GaussAsymmetric:
    grad = &ggaussasymmetric;
    cerr <<"GradientGaussAsymmetric\n";
    break;
  default:
    BIASERR("unknown gradient type");
    grad = NULL;
    break;
  }
  // activate some debugging output of gradient object
  //grad->AddDebugLevel(D_GRADGAUSS_KERNEL);
  //grad->AddDebugLevel(D_CONV_KERNEL);

  // get the point to track from command line
  p[0][2]=p[1][2]=1.0;
  p[0][0]=atof(argv[argind]);
  p[0][1]=atof(argv[argind+1]);
  cout << "attempt to track point "<<p[0] << endl;


  // load image
  if (LoadImage(argv[argind+2], im[0])!=0){
    BIASERR("error loading image "<<argv[argind+2]);
    return -1;
  }

  // calculate gradients and low pass filtered images
  binomial.Filter3x3(im[0], fim[0]);
  grad->Filter(fim[0], gradx[0], grady[0]);


  counter=argind+2;
  newim=0;
  // now loop through the images
  while (counter < argc-1){
    counter++;
    newim=(newim==0)?(1):(0);
    oldim=(newim==0)?(1):(0);

    if (LoadImage(argv[counter], im[newim])!=0){
      BIASERR("error loading image "<<argv[counter]);
      return -1;
    } else {
      if (debug)
        cout << "\n------- newim: " << argv[counter]
             << "\n------- oldim: " << argv[counter-1] << endl;

      md=im[newim].GetMetaData();
      if (md->Find(AppData::MD_HomgPoint2D, "#[HomgPoint2D]", appdata)>=0){
        if (appdata.tag==AppData::MD_USE_ASCII){
          if (debug) cerr << appdata.stag <<" : "<<appdata.sdata<<endl;
          sscanf(appdata.sdata.c_str(), "[ %lf %lf %lf ]",
                 &realpoint[0], &realpoint[1], &realpoint[2]);
        } else {
          memcpy(realpoint.GetData(), appdata.data, appdata.length);
        }
        if (debug) cerr <<"realpoint is at "<<realpoint<<endl;
      } else {
        BIASERR("cannot find HomgPoint2D in meta data");
      }
    }

    // calculate gradients and low pass filtered images
    binomial.Filter3x3(im[newim], fim[newim]);
    grad->Filter(fim[newim], gradx[newim], grady[newim]);

    // time the tracking
    timer.Start();
    tc.Start();

#ifdef BIAS_DEBUG
    //ImageIO::Save("im1.mip", im[oldim]);
    //ImageIO::Save("im2.mip", im[newim]);
    //ImageIO::Save("fim1.mip", fim[oldim]);
    //ImageIO::Save("fim2.mip", fim[newim]);
    //ImageIO::Save("gx1.mip", gradx[oldim]);
    //ImageIO::Save("gx2.mip", gradx[newim]);
    //ImageIO::Save("gy1.mip", grady[oldim]);
    //ImageIO::Save("gy2.mip", grady[newim]);
    ImageIO::Save("im1.mip", im[oldim]);
    ImageIO::Save("im2.mip", im[newim]);
    ImageIO::Save("fim1.mip", fim[oldim]);
    ImageIO::Save("fim2.mip", fim[newim]);
    ImageIO::Save("gx1.mip", gradx[oldim]);
    ImageIO::Save("gx2.mip", gradx[newim]);
    ImageIO::Save("gy1.mip", grady[oldim]);
    ImageIO::Save("gy2.mip", grady[newim]);
    cerr << "Tracking from "<<p[oldim]<<"  -->  "<<p[oldim]<<endl;
#endif

    // first: initialize the tracker
    tracker->Init(fim[oldim], fim[newim], gradx[oldim], grady[oldim],
                  gradx[newim], grady[newim]);

    // second: do the tracking
    res =
      tracker->Track(p[oldim], p[oldim], p[newim], error, iter,
                    residuumMAD, residuumMSD, cov);

    // time the tracking
    tc.Stop();
    timer.Stop();
    tc.Print();
    tc.Reset();

    // check for errors
    if (res<0){
      // stop looping through images if point vanishes from the valid
      // image region
      if (res==-2){
        cout << "point ("<<p[oldim][0]<<", "<<p[oldim][1]<<") too close to "
            <<"image border, stopping"<<endl;
         break;
      }
      BIASERR("error matching "<<res);
      assert(false);
    }

    // calculate the error with respect to ground truth from image header
    err=sqrt((p[newim][0]-realpoint[0])*(p[newim][0]-realpoint[0])+
             (p[newim][1]-realpoint[1])*(p[newim][1]-realpoint[1]));
    errorsum+=err;

    // draw and save the result
    if (draw)
      Draw(im[newim], p[newim], cov, argv[counter]);

    cout << setw(2) << counter-3 << " : (" << p[oldim][0]<<", "<<p[oldim][1]
         << ") -> ("<< p[newim][0]<<", "<<p[newim][1]<<")  klt-error: "<<error
         <<"   real: ("<<realpoint[0]<<", "<<realpoint[1]
         <<")   dist-err: "<<err<<"   residuumMAD: "<<residuumMAD
         <<"   residuumMSD: "<<residuumMSD<<endl;

  }
  // print out some statistics
  cerr << "errorsum over "<<counter-3<<" images: "<<errorsum<<endl
       << "  mean error "<<errorsum/(double)(counter-3)<<endl;

  timer.Print();
  cerr << "KLT trackin over "<<argc-3<<" images took "<<timer.GetRealTime()
       <<" us"
       << "\n  this is equal to "<<timer.GetRealTime()/double(argc-3)
       <<" us per image and corner" << endl;
  para.DisableDestructorWarning();
  return 0;
}