ExampleTracker.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 ExampleTracker.cpp
    @brief This little example demonstrates the most simple case of usage of the
    Tracker class. It also demonstrates that it is neccessary to extract the
    tracking information and store them externally.
    @ingroup g_examples
    @author woelk */


#include <bias_config.h>

#include <Base/Common/BIASpragma.hh>

#include <Matcher2D/Tracker.hh>
#include <Base/ImageUtils/ImageDraw.hh>
#include <Base/Image/ImageConvert.hh>
#include <Base/Image/ImageIO.hh>
#include <Image/PyramidImage.hh>
#include <FeatureDetector/CornerDetectorKLT.hh>
#include <Base/Math/Random.hh>

#define TIMING
#ifdef TIMING
#include <Base/Debug/TimeMeasure.hh>
#endif

#include <deque>

using namespace BIAS;
using namespace std;


#define StorageType float
#define CalculationType float

//#define DEBUG false
#define DEBUG 1

//  the resulting data structs
deque<vector<HomgPoint2D> > p; //  the corners
deque<vector<KLT_TYPE> > residui; // mean absolute grey difference
/* int representing success (=0) or failure reason (<0) see
   TrackerBase::Track() for details */
deque<vector<int> > results;
// covariance matrices
deque<vector<Matrix<double> > > cov;

// temporary vector for the
vector<HomgPoint2D> mp;
vector<QUAL> qual; // the displacement in the last iteration step
vector<HomgPoint2D> cp;
vector<KLT_TYPE> r; //
vector<int> res;
vector<Matrix<double> > tmp_cov;

ImageBase::EStorageType st_type=ImageBase::ST_invalid;

const int max_num_points = 150;

/** Draw the tracks with a circle around the actual corner position.
    The covariances are visualized as ellipses. */
void Draw(Image<StorageType>& stim, Image<unsigned char>& im,
          deque<vector<HomgPoint2D> >& p,
          deque<vector<Matrix<double> > >& cov, int imnum)
{
  // should the tracks be drawn as trailing lines ?
  bool draw_tracks=true;
  bool draw_cov=false;

  // draw the 3 sigma ellipsoid scaled with factor 100
  double cov_scale=3e2;

  Image<unsigned char> ucim;

  if (st_type==ImageBase::ST_unsignedshortint){
    Image<StorageType> scaled = stim;
    scaled.ScaleShift(255.0/4095.0, 0.0);
    ImageConvert::ConvertST(scaled, ucim, ImageBase::ST_unsignedchar);
  } else {
    ImageConvert::ConvertST(stim, ucim, ImageBase::ST_unsignedchar);
  }
  ImageConvert::Convert(ucim, im, ImageBase::CM_RGB);

  unsigned char col[]={255, 0, 0};
  unsigned char colgreen[]={0, 255, 0};
  const int nump=p[0].size();
  unsigned start[2], end[2];
  const unsigned radius=1;
  int maxhist=0;
  double center[2], a[2], b[2], eva, evb;
  Vector2<double> na, nb;

  if (imnum>0){
    for (int l=0; l<nump; l++){
      if (!p[imnum][l].IsAtInfinity() && !p[imnum-1][l].IsAtInfinity()){
        if (draw_cov){
          center[0]=p[imnum][l][0];
          center[1]=p[imnum][l][1];
          Matrix2x2<KLT_TYPE> curcov;

          // we will get a nxn covariance matrix, depending on the number
          // of parameters. dx,dy is always the last rows/cols in the
          // covariance matrix, therefore copy these data for vizualization
          int covoffset =  cov[imnum][l].GetRows()-2;
          for (int row=0; row<2; row++)
            for (int col=0; col<2; col++)
              curcov[row][col] = cov[imnum][l][row+covoffset][col+covoffset];

          curcov.EigenvalueDecomposition(eva, na, evb, nb);
          // eigenvalues are the \sigma^2
          eva=sqrt(eva);
          evb=sqrt(evb);
          //out << 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;
          //cout<<"drawing ellipse at "<<center<<" with a="<<a[0]<<" "
          //    <<a[1]<<" b="<<b[0]<<" "<<b[1]<<endl;
          ImageDraw<unsigned char>::
            Ellipse(im, center, a, b, col);
        }
        // circle around corner in actual image
        start[0]=(unsigned)rint(p[imnum][l][0]);
        start[1]=(unsigned)rint(p[imnum][l][1]);
        ImageDraw<unsigned char>::CircleCenterFilled(im, start[0], start[1],
                                                     radius, col);

        if (draw_tracks){
          // lines for tracks
          for (int i=imnum; i>0; i--){
            if (p[i-1][l].IsAtInfinity()) {
              if ((imnum-i)>maxhist) maxhist=imnum-i;
              break;
            }
            start[0]=(unsigned)rint(p[i][l][0]);
            start[1]=(unsigned)rint(p[i][l][1]);
            end[0]=(unsigned)rint(p[i-1][l][0]);
            end[1]=(unsigned)rint(p[i-1][l][1]);
            if (i-1==0) maxhist=imnum;
            ImageDraw<unsigned char>::Line(im, start, end, col);
          }
        }

      }
    }
  } else {
    // draw initial poinst blue
    for (int l=0; l<nump; l++){
      if (!p[imnum][l].IsAtInfinity()){
        // circle around corner in actual image
        start[0]=(unsigned)rint(p[imnum][l][0]);
        start[1]=(unsigned)rint(p[imnum][l][1]);
        ImageDraw<unsigned char>::CircleCenterFilled(im, start[0], start[1],
                                                     radius, colgreen);
      }
    }
  }
}


/** print tracks to stdout, just for debugging purposes */
void DumpTracks(deque<vector<HomgPoint2D> >& p)
{
  int numim=0;
  while (p[numim].size()>0 && numim<(int)p.size()) numim++;
  for (unsigned l=0; l<p[0].size(); l++){
    cout <<setw(3)<<l<<" : ";
    for (int i=0; i<numim; i++){
      cout.precision(4);
      fprintf(stdout, "%i:(%5.1f, %5.1f) ", i, p[i][l][0], p[i][l][1]);
    }
    cout << endl;
  }
}

// the drawing algorithm needs one point at infinity between subsequent tracks,
// therefore the last point of the last track is set to (0, 0, 0)
void InsertZeroPoints(deque<vector<HomgPoint2D> >& p, int i)
{
  if (i>0){
    const int nump=p[i].size();

    for (int k=0; k<nump; k++){
      if (p[i][k].IsAtInfinity() && !p[i-1][k].IsAtInfinity()){
        p[i-1][k].Set(0.0, 0.0, 0.0);
      }
    }
  }
}

/** The main function */
int main(int argc, char *argv[])
{
  bool prefilter = true;
  bool testBrightnessVariation = true;

  ImageBase baseim;
  Image<unsigned char> rgbim;
  Image<StorageType> im, im2;

  // allocate the tracker
  Tracker<StorageType, CalculationType> tr(TT_Simple);
  //tr.AddDebugLevel(D_TRACKER_INIT);
  //tr.AddDebugLevel(D_TRACKER_PARAM);
  //tr.AddDebugLevel(D_TRACKER_TRACK);
  //tr.AddDebugLevel(D_TRACKER_WRITE_IM);
  //tr.AddDebugLevel(D_TRACKER_FILL_UP);
  Vector<int> hws(3);
  hws.newsize(5);
  hws[4] =hws[3] = hws[2] =hws[1] =hws[0] = 10;
  tr.SetHalfWinSize(hws);
  tr.SetRejectionType(-1);
  tr.SetAffineBrightnessInvariance(testBrightnessVariation);

  Vector<int> maxit(3);
  maxit.newsize(5);
  maxit[4] =maxit[3] = maxit[2] =maxit[1] =maxit[0] = 50;
  tr.SetMaxIterations(maxit);


  // the pyramid images used by the tracker
  PyramidImage<CalculationType> *pim[2]; // low pass filtered images
  PyramidImage<CalculationType> *gx[2]; // horizontal gradient
  PyramidImage<CalculationType> *gy[2]; // vertical gradient
  // allocate the pyramid images

  for (int i=0; i<2; i++){
    pim[i] = new PyramidImage<CalculationType>;
    gx[i] = new PyramidImage<CalculationType>;
    gy[i] = new PyramidImage<CalculationType>;
  }

  // allocate the corner detector
  CornerDetectorKLT<StorageType, CalculationType> cd;
  //cd.AddDebugLevel(D_CD_WRITE_DEBUG_IM);
  //cd.AddDebugLevel(D_CD_FEATURES);

  int argind = 1;

  // check if enough images were given on command line
  if (argc-argind<2 || argind<1){
    cerr << argv[0] << " [parameter] <im1> <im2> [ <im3> ... ] \n";
    return -2;
  }

  // fill up lost tracks every cdfreq-th frame
  //int cdfreq=10;
  // separate the pyramid images
  int act=0;
  // how many corners are successfully tracked
  int num=0;

#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  Random R;
  // loop for every image
  for (int i=argind; i<argc; i++){
    // act is the index into the actual pyramid image
    // toggle act between 0 and 1
    act=!act;

    if (ImageIO::Load(argv[i], baseim)!=0){
      BIASERR("error loading image "<<argv[i]);
      return -1;
    } else {
      if (DEBUG) cerr << "read "<<argv[i]<<endl;
#ifdef TIMING
      cerr << "---------------"<<argv[i]<<"---------------\n";
#endif
    }
    st_type=baseim.GetStorageType();

    // convert to float
    if (ImageConvert::ConvertST(baseim, im, ImageBase::ST_float)!=0){
      BIASERR("error converting image "<<argv[i]);
    }

    // convert to grey if necessairy
    if (im.GetChannelCount()!=1){
      if (ImageConvert::ToGrey(im, im)!=0){
        BIASERR("error converting image to grey"<<argv[i]);
      }
    }
    if (testBrightnessVariation) {
      double scale = R.GetUniformDistributed(0.7, 1.3);
      double shift = R.GetUniformDistributed(-30,30);
      cout<<"scaling with "<<scale<<"  and shifting "<<shift<<endl;
      im.ScaleShift(scale, shift);
      float* pimdata = im.GetImageData();
      for (unsigned int i=0; i<im.GetPixelCount(); i++) {
        if (*pimdata<0.0) *pimdata = 0.0;
        if (*pimdata>255.0) *pimdata = 255.0;
        pimdata++;
      }
      //ImageIO::Save("scaleshift.mip",im);
      ImageIO::Save("scaleshift.mip",im);
    }

#ifdef TIMING
    timer.Stop();
    cerr << "loading and converting to float/grey took "<<timer.GetRealTime()<<" us\n";
    timer.Reset();
    timer.Start();
#endif

    if (prefilter)
    {
      // the first thing that needs to be done is to calculate
      // the pyramid images
      tr.PreparePyramide(im, *pim[act], *gx[act], *gy[act]);
    }
    else
    {
      tr.PreparePyramide(im, *pim[act]);
    }

#ifdef TIMING
    timer.Stop();
    cerr << "pyramid preparation took "<<timer.GetRealTime()<<" us\n";
    timer.Reset();
    timer.Start();
#endif

    // secondly the tracking process is started
    if (prefilter)
    {
      num=tr.Track(*pim[act], *gx[act], *gy[act]);
    }
    else
    {
      num=tr.Track(*pim[act]);
    }

#ifdef TIMING
    timer.Stop();
    cerr << "tracking took "<<timer.GetRealTime()<<" us for "<<num<<" corners\n";
    cerr << " = "<<timer.GetCycleCount()/num<<" fps/corner/frame\n";
    timer.Reset();
    timer.Start();
#endif

    // now initially tell the tracker where to track
    if (i==argind){
      if (!prefilter)
      {
        tr.PreparePyramide(im, *pim[act], *gx[act], *gy[act]);
      }
      cd.Detect(*(*gx[act])[0], *(*gy[act])[0], mp, qual);
      cout << "initially found "<<mp.size()<<" points\n";

      // let the tracker know of the point positions
      //tr.AddNumPoints(mp, qual, max_num_points);

      if ((int)mp.size() > (int)max_num_points)
      {
        mp.erase(mp.begin() + max_num_points, mp.end());
        qual.erase(qual.begin() + max_num_points, qual.end());
      }
      tr.ReplaceAllPoints(mp, qual);

#ifdef TIMING
      timer.Stop();
      cerr << "corner detection took "<<timer.GetRealTime()<<" us\n";
      timer.Reset();
      timer.Start();
#endif
    }

    // remember the corners in the global data struct
    tr.GetPoints(mp);
    p.push_back(mp);

    // now remember the residui (mean grey value difference)
    tr.GetResiduiMAD(r);
    residui.push_back(r);
    // and the success indicator
    tr.GetResults(res);
    results.push_back(res);

    tr.GetCovariances(tmp_cov);
    cov.push_back(tmp_cov);

#ifdef TIMING
    timer.Stop();
    cerr << "get results took "<<timer.GetRealTime()<<" us\n";
    timer.Reset();
    timer.Start();
#endif

/*
    // at every cdfreq-th frame the lost tracks indicated by points at infinity
    // (i.e. p[2]==0) are filled up again
    if (i!=argind && ((i-argind) % cdfreq)==0){

      if (!prefilter)
      {
        tr.PreparePyramide(im, *pim[act], *gx[act], *gy[act]);
      }

      // We are not interested in corners already tracked,
      // hence the corner detector must know about the points which are
      // already tracked
      tr.GetPoints(cp);

      cd.Detect(*(*gx[act])[0], *(*gy[act])[0], cp, qual);
      // the track length in the globals data structs are determiend
      // by 'zero points'
      InsertZeroPoints(p, i-argind);

      // now only the points where tracking failed are filled up
      int num=tr.FillUpPoints(cp, qual);
      if (DEBUG) cout << "filled up "<<num<<" points\n";
      // get the newly filled up points and remember them in the global deque
      tr.GetPoints(mp);
      // restrict the maximum track length in the global data struct (optional)
      p.pop_back();
      p.push_back(mp);

    }
*/

#ifdef TIMING
    timer.Stop();
    cerr << "corner detection took "<<timer.GetRealTime()<<" us\n";
    timer.Reset();
    timer.Start();
#endif


    // now draw the tracks into rgbim
    Draw(*(*pim[act])[0], rgbim, p, cov, i-argind);
    // write result image to disk
    ostringstream name;
    name << "track-"<<setw(4)<<setfill('0')<<i-argind<<".mip";
    //ImageIO::Save(name.str(), rgbim);
    ImageIO::Save(name.str(), rgbim);


#ifdef TIMING
    timer.Stop();
    cerr << "drawing/writing took "<<timer.GetRealTime()<<" us\n";
    timer.Reset();
    timer.Start();
#endif
  }

  /*
  // write a Birchfeld style tracker file
  if (DEBUG) tr.WriteTrackFile(p, results, "features.txt");

  if (DEBUG) cout << "\nTracks: "<<endl;
  if (DEBUG) DumpTracks(p);
  if (DEBUG) cout << "\n";
  */

  return 0;
}