CondensImg.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 "CondensImg.hh"
#include <Base/ImageUtils/ImageDraw.hh>

using namespace BIAS;
using namespace std;

void CondensImg::InitModelDefaults()
{
  // the process mean is the image center
  processMean_=areaMax_ * 0.5;

  // this draws the sample to the image center a little
  // its 99% the old position, 1% the mean
  processFirstOrderScale_.Set(0.99);

  // a second Order ARP, set scale to zero to have just a first order
  // this takes 90% of the last displacements (velocity)
  processSecondOrderScale_.Set(.9);

  // and a diffusion
  diffusionSigma_=0.01 * areaMax_;

  importanceFraction_ = 0.0;

  defaultInitFraction_=0.05;
}


void CondensImg::InitPriorPositions(unsigned int nrOfInitSamples)
{
  // uniform distribution
  for (unsigned int n=0; n<nrOfInitSamples; n++)
    for (unsigned int d=0;d<stateDim_;d++) {
      samplePosNew_[n][d]=
        random_.GetUniformDistributed(areaMin_[d],areaMax_[d]);
    }
}


void CondensImg::EvaluateObservationDensities()
{
  // this function has to assign a weight (probability) to each sample
  for (unsigned int n=0;n<Nsamples_;n++) {
    // the observation is set by SetObservation
    // just set the image value as weight
    if ((samplePosNew_[n][0]>0) &&
        (samplePosNew_[n][0]<= (double)obsImg_.GetWidth()) &&
        (samplePosNew_[n][1]>0) &&
        (samplePosNew_[n][1]<=  (double)obsImg_.GetHeight()) ) {
      sampleWeights_[n]=
        (double)(obsImg_.GetImageDataArray()[(int)samplePosNew_[n][1]][(int)samplePosNew_[n][0]]);
    }
  }
}

void CondensImg::EvaluateImportanceWeights()
{
  // assume we know, that the 'real' object is in the left image half
  // assign importanceWeights accordingly
  for (unsigned int n=0; n<Nsamples_; n++)
    if ( (samplePosOld_[n][0]>areaMin_[0]) &&
         (samplePosOld_[n][1]>areaMin_[1]) &&
         (samplePosOld_[n][0]<areaMax_[0]/2) &&
         (samplePosOld_[n][1]<areaMax_[1]) )
      sampleImportanceWeights_[n]=255;
    else sampleImportanceWeights_[n]=0;
}

void CondensImg::SetArea(Vector2<double> min, Vector2<double> max)
{
  areaMin_=min;
  areaMax_=max;
}


void CondensImg::SetObservation(Image<unsigned char> &obsImg)
{
  obsImg_=obsImg;
}

void CondensImg::DrawSamples(Image<unsigned char> &img, unsigned char value)
{
  if (!img.IsEmpty()) img.Release();
  img.Init(obsImg_.GetWidth(),obsImg_.GetHeight(),1);
  img.SetColorModel(ImageBase::CM_Grey);
  img.FillImageWithConstValue((unsigned char)0);
  unsigned int radius;
  unsigned char values[1];
  for (unsigned int i=0;i<Nsamples_;i++) {
    values[0] = value;
    radius = (unsigned int)(sampleWeights_[i]* Nsamples_ /
                           sumOfWeights_ + 0.5);
    //   cerr<<"Drawing circle with radius:"<<radius<<
    //      "at pos:"<<samplePosOld_[i]<<endl;
    if ( (samplePosOld_[i][0]-radius>0) && (samplePosOld_[i][1]-radius>0) &&
         (samplePosOld_[i][0]+radius<img.GetWidth()) &&
         (samplePosOld_[i][1]+radius<img.GetHeight()) )
      ImageDraw<unsigned char>::CircleCenter(img,
                                             (unsigned int)samplePosOld_[i][0],
                                             (unsigned int)samplePosOld_[i][1],
                                             radius, &values[0]);
  }
}


void CondensImg::DrawPosteriorDistribution(BIAS::Image<unsigned char> &img)
{
  if (fImg_.IsEmpty())
    fImg_.Init(img.GetWidth(), img.GetHeight(),1);

  double max = DrawPosteriorDistribution(fImg_);

  float *pixel;
  // now copy values into result img.
  pixel = fImg_.GetImageData();
  unsigned char *data= img.GetImageData();
  unsigned int size=img.GetPixelCount();
  for (unsigned int i=0;i<size;i++) {
    *data = (unsigned char)(*pixel / max  * 255.0);
    data++;
    pixel++;
  }
}




double CondensImg::DrawPosteriorDistribution(BIAS::Image<float> &img)
{
  float max=0;
  float *pixel;
  double x,y,dx,dy;
  int width=img.GetWidth() ,height=img.GetHeight();
  img.FillImageWithConstValue((float)0);
  for (unsigned int i=0;i<Nsamples_;i++) {
    x = samplePosOld_[i][0];
    y = samplePosOld_[i][1];

    if ( (x>0) && (y>0) &&
         (x<width-1) && (y<height-1) ) {
      pixel= (img.GetImageDataArray()[(int)y])+ (int)x;
      // inverse bilinear interpolation
      dx = x - (double)floor(x);  dy = y - (double)floor(y);
      // top left
      *pixel+= float((1-dx) * (1-dy) * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
      // top right
      pixel++;
      *pixel+= float((1-dy) * dx * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
      // bottom right
      pixel+=width;
      *pixel+= float(dx * dy * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
      // bottom left
      pixel--;
      *pixel+= float((1-dx) * dy * sampleWeights_[i]);
      if (*pixel>max) max=*pixel;
    }
  }
  return max;
}


void CondensImg::DrawMean(BIAS::Image<unsigned char> &img)
{
  double var=0;
  BIAS::Vector<double> diff;
  for (unsigned int n=0;n<Nsamples_;n++) {
    diff = samplePosOld_[n] - mean_;
    var +=  diff.ScalarProduct(diff) * sampleWeights_[n];
  }
  var /= sumOfWeights_;
  var = sqrt(var);
  ImageDraw<unsigned char>::LineGrey(img, (int)(mean_[0]-var),
                                     (int)(mean_[1]),  (int)(mean_[0]+var),
                                      (int)(mean_[1]),255);
  ImageDraw<unsigned char>::LineGrey(img,  (int)mean_[0],  (int)(mean_[1]-var),
                                     (int)(mean_[0]), (int)(mean_[1] + var), 255);
}