Condensation.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 "Condensation.hh"
#include <float.h>
#include <Base/Common/W32Compat.hh>

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

using namespace BIAS;
using namespace std;

Condensation::Condensation()
{
  stateDim_=0;
  defaultInitFraction_=0.1; 
  importanceFraction_=0.0;
  _doSecondOrderARP=true;
  _randomArray=NULL;
  NewDebugLevel("D_COND_INIT");
  NewDebugLevel("D_COND_PROCESS");
  NewDebugLevel("D_COND_WEIGHTS");
  NewDebugLevel("D_COND_DEBUG");
  NewDebugLevel("D_COND_ERROR");
  NewDebugLevel("D_COND_MEANVAR");
}

Condensation::~Condensation()
{
  if (_randomArray) delete[] _randomArray;
}

int Condensation::Init(unsigned int N)
{
  initial_=true;
  Nsamples_=N;
  sumOfWeights_=0;
  random_.Reset();
  _randomArray=new double[N];
  // memory allocation
  samplePosNew_.resize(N);
  samplePosOld_.resize(N);
  samplePosTminus2_.resize(N);
  indexTminus2_.resize(N);
  oldIndices_.resize(N);
  sampleWeights_.resize(N);
  sampleWeights_.assign(N,1);
  cumulProbArray_.resize(N);
  sampleImportanceWeights_.resize(N);
  cumulProbArrayImportance_.resize(N);
  correctionOfImportanceWeights_.resize(N);  
  correctionOfImportanceWeights_.assign(N,1);  

  if (stateDim_==0) {
    BIASERR("Error at Init: stateDim_==0, set this at constructor");
    return -1;
  }
  
  // further memory allocation
  diffusionSigma_.newsize(stateDim_);
  diffusionSigma_.Set(0);
  processFirstOrderScale_.newsize(stateDim_);
  processFirstOrderScale_.Set(0);
  processSecondOrderScale_.newsize(stateDim_);
  processSecondOrderScale_.Set(0);
  processMean_.newsize(stateDim_);
  processMean_.Set(0);
  offSet_.newsize(stateDim_);
  offSet_.Set(0);
  mean_.newsize(stateDim_);
  mean_.Set(0);
  meanTminus1_.newsize(stateDim_);
  meanTminus1_.Set(0);
  BCDOUT(D_COND_INIT,"allocating memory for sample vectors\n");

  InitModelDefaults();
  BCDOUT(D_COND_INIT,"InitModelDefaults() passed.\n");

  // initial positions
  for (unsigned int n=0;n<Nsamples_;n++) {
    samplePosNew_[n].newsize(stateDim_);
    samplePosOld_[n].newsize(stateDim_);
    samplePosTminus2_[n].newsize(stateDim_);
  }
  return 0;
}

int Condensation::InitPrior()
{
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  BCDOUT(D_COND_INIT,"Initial Positions set.\n"); 
  InitPriorPositions(Nsamples_); 
  samplePosOld_ = samplePosNew_;
  samplePosTminus2_ = samplePosOld_; 
  sumOfWeights_=0;
  for (unsigned int n=0; n<Nsamples_; n++) {
    BCDOUT(D_COND_INIT,"prior position of sample "<<n<<": "
              <<samplePosOld_[n]<<endl);
    sumOfWeights_ +=  sampleWeights_[n];
    cumulProbArray_[n] = sumOfWeights_;
    indexTminus2_[n] = n;
  }
  BCDOUT(D_COND_INIT,"init done\n");
#ifdef TIMING
  timer.Stop();
  cerr << "Condensation::InitPrior()  took "<<timer.GetRealTime()<<" us"<<endl;
#endif

  return 0;
}



int Condensation::PredictNewSamplePositions()
{
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif

  unsigned int sampleIndex=0;
  // the number of samples distributed by PriorPositions
  // samples with index < initSamples are distributed in this way
  unsigned int initSamples=(unsigned int)((double)Nsamples_*
                                          (defaultInitFraction_));
  // samples with index < importanceSamples and index >= initSamples
  // are distributed using importance sampling
  unsigned int importanceSamples=(unsigned int)((double)Nsamples_*
                                                importanceFraction_+
                                                initSamples);

  InitPriorPositions(initSamples);

#ifdef TIMING
  timer.Stop();
  cerr << "Condensation::PredictNewSamplePositions()  pick prior samples took "<<timer.GetRealTime()<<" us"<<endl;
  timer.Reset();   timer.Start();
#endif


  if (importanceSamples!=initSamples && importanceSamples!=0) {
    //// importance sampling
    EvaluateImportanceWeights();

    sumOfImportanceWeights_ = 0;
    for (unsigned int n=0; n<Nsamples_; n++) {
      BCDOUT(D_COND_WEIGHTS,"importance weight for sample "<<n<<": "
                <<sampleImportanceWeights_[n]<<endl);
      sumOfImportanceWeights_ += sampleImportanceWeights_[n];
      cumulProbArrayImportance_[n] = sumOfImportanceWeights_;
    }
    for (unsigned int i=initSamples; i<importanceSamples; i++) {
      BCDOUT(D_COND_PROCESS,"importance sample with index:"
                <<sampleIndex<<endl);    
      sampleIndex = PickImportanceSample_();
      //      indexTminus2_[i]=sampleIndex;
      oldIndices_[i]=sampleIndex;
      SecondOrderARP_(sampleIndex, samplePosNew_[i]);        
      correctionOfImportanceWeights_[i]=
        sampleWeights_[sampleIndex] /
        (sampleImportanceWeights_[sampleIndex]+1E-9);
      BCDOUT(D_COND_PROCESS,"adding diffusion for sampleIndex:"
                <<sampleIndex<<endl);
    }
  }

#ifdef TIMING
  timer.Stop();
  cerr << "Condensation::PredictNewSamplePositions()  pick importance samples took "<<timer.GetRealTime()<<" us"<<endl;
  timer.Reset();   timer.Start();
#endif

  //// standard
  /*
  int num=Nsamples_-importanceSamples;
  random_.GetUniformDistributed(0.0, sumOfWeights_, num, _randomArray);
  double r;
  int p;

  // fast
  qsort((void*)_randomArray, num, sizeof(double), Condensation::compdouble);
  p=0;
  if (_doSecondOrderARP){
    for (int n=0; n<num; n++){
      r=_randomArray[n];
      while (cumulProbArray_[p]<r) p++;
      indexTminus2_[n]=p;
      SecondOrderARP_(sampleIndex, samplePosNew_[n]); 
    }
  } else {
    for (int n=0; n<num; n++){
      r=_randomArray[n];
      while (cumulProbArray_[p]<r) p++;
      samplePosNew_[n]=samplePosOld_[p];
    }
  }
  */
  // this is astonishingly faster ?
  /// now pick samples from the last time step using the weights
  for (unsigned int n=importanceSamples; n<Nsamples_; n++) {   
    sampleIndex = PickOneSample_();
    BCDOUT(D_COND_PROCESS,"predicting sample with index:"<<sampleIndex<<endl);    
    oldIndices_[n]=sampleIndex;
      //indexTminus2_[n]=sampleIndex;
    BCDOUT(D_COND_DEBUG,"Nsamples in SecondOrderARP: "<<n<<" SampleIndex: "
              <<sampleIndex<<endl);
    SecondOrderARP_(sampleIndex, samplePosNew_[n]);                      
  }
  indexTminus2_=oldIndices_;

#ifdef TIMING
  timer.Stop();
  cerr << "Condensation::PredictNewSamplePositions()  pick standard samples took "<<timer.GetRealTime()<<" us"<<endl;
  timer.Reset();   timer.Start();
#endif

  /// most probably from the process model
  AddOffsets_();

  /// most probably gaussian noise
  AddDiffusions_();

#ifdef TIMING
  timer.Stop();
  cerr << "Condensation::PredictNewSamplePositions()  took "<<timer.GetRealTime()<<" us"<<endl;
#endif

  return 0;
}


void Condensation::SetDefaultInitFraction(double fraction)
{ 
  if (fraction<0) defaultInitFraction_=0;
  else if (fraction>1) defaultInitFraction_=1;
  else defaultInitFraction_=fraction;
}


void Condensation::SetImportanceSampleFraction(double fraction)
{
  if (fraction<0) importanceFraction_=0;
  else if (fraction>1) importanceFraction_=1;
  else importanceFraction_=fraction;
}

void Condensation::AddDiffusions_()
{  
  // do not add diffusion to newly generated samples
  const unsigned StartIndex=
    (unsigned int)((double)Nsamples_*defaultInitFraction_);
 
  std::vector<double> r;
  r.reserve(Nsamples_-StartIndex);
  for (unsigned int d=0; d<stateDim_; d++) {
    random_.GetNormalDistributed(0.0, diffusionSigma_[d], 
                                 Nsamples_-StartIndex, r);
    for (unsigned i=StartIndex; i<Nsamples_; i++){
      samplePosNew_[i][d] += r[i-StartIndex];
    }
  }
}

void Condensation::AddOffsets_()
{
  if (offSet_.size()>0)
    for (unsigned int n=0;n<Nsamples_;n++) {
      samplePosNew_[n] += offSet_;
      samplePosOld_[n] += offSet_;
    }
}

void Condensation::CalculateBaseWeights_()
{
#ifdef TIMING
  TimeMeasure timer;
  timer.Start();
#endif
  double cumul_total;

  cumul_total = 0.0;
  EvaluateObservationDensities();

#ifdef TIMING
  timer.Stop();
  std::cout<< "Condensation::CalculateBaseWeights_() Eavl took "<<timer.GetRealTime()<<" us"<<std::endl;
  timer.Reset();
  timer.Start();
#endif

  for (unsigned int n=0; n<Nsamples_; n++) {
    BCDOUT(D_COND_WEIGHTS,"weight for sample "<<n<<": "<<sampleWeights_[n]<<endl);
    cumul_total += sampleWeights_[n];
    cumulProbArray_[n] = cumul_total;
  }
  sumOfWeights_ = cumul_total;

#ifdef TIMING
  timer.Stop();
  std::cerr << "Condensation::CalculateBaseWeights_()  took "<<timer.GetRealTime()<<" us"<<std::endl;
#endif
}

void Condensation::Process()
{ 
#ifdef TIMING
  TimeMeasure timer, ot;
  timer.Start();
  ot.Start();
#endif

  BCDOUT(D_COND_PROCESS,"Calculating Weights\n");
  CalculateBaseWeights_();

#ifdef TIMING
  timer.Stop();
  std::cerr << "Condensation::Process() CalculateBaseWeights_() took "<<timer.GetRealTime()<<" us"<<std::endl;
  timer.Reset();
  timer.Start();
#endif

  samplePosTminus2_=samplePosOld_;
  samplePosOld_=samplePosNew_;
  meanTminus1_=mean_;

#ifdef TIMING
  timer.Stop();
  std::cerr << "Condensation::Process() copy of vector took "<<timer.GetRealTime()<<" us"<<std::endl;
  timer.Reset();
  timer.Start();
#endif

  CalculateMean_();
  if (initial_) {
    meanTminus1_=mean_;  
    initial_=false;
  }

#ifdef TIMING
  timer.Stop();
  std::cerr << "Condensation::Process() calculate mean took "<<timer.GetRealTime()<<" us"<<std::endl;
  ot.Stop();
  std::cerr << "Condensation::Process() took "<<ot.GetRealTime()<<" us"<<std::endl;
#endif
}

double Condensation::GetVariance() const
{
  BIASASSERT(!isnan(sumOfWeights_) && !isinf(sumOfWeights_));
  if (sumOfWeights_<1E-20||sumOfWeights_!=sumOfWeights_) return 0; 
  double var=0;
  BIAS::Vector<double> diff;
  for (unsigned int n=0;n<Nsamples_;n++) {
    diff = samplePosOld_[n] - mean_;    
    var +=  (diff*diff) * sampleWeights_[n];
  }
  var /= sumOfWeights_;  
  if (var!=var) return 0;
  return var;  
}

bool Condensation::GetWeightedVariance(Vector<double>& var) const
{
  var.newsize(stateDim_);
  var.SetZero();
  unsigned start=(unsigned)rint(Nsamples_*(defaultInitFraction_+
                                           importanceFraction_));
  double SumOfWeights=cumulProbArray_[Nsamples_-1]-cumulProbArray_[start];
  if (SumOfWeights<DBL_EPSILON) {
    BCDOUT(D_COND_MEANVAR, "sum of weights top small for variance "
              <<"calculation "<<SumOfWeights);
    return false; 
  }
  Vector<double> diff;
  for (unsigned int n=start;n<Nsamples_;n++) {
    diff = samplePosOld_[n] - mean_; 
    //diff *= sampleWeights_[n];
    var += (diff.ElementwiseProduct(diff)) * sampleWeights_[n];
  }
  var /= SumOfWeights;
  return true;
}

bool Condensation::GetVariance(Vector<double>& var) const
{
  var.newsize(stateDim_);
  var.SetZero();
  unsigned start=(unsigned)rint(Nsamples_*(defaultInitFraction_+
                                           importanceFraction_));
  if (Nsamples_-start<=1) {
    BCDOUT(D_COND_MEANVAR, "not enough samples "<<Nsamples_-start<<endl);
    return false; 
  }
  Vector<double> diff;

  for (unsigned int n=start;n<Nsamples_;n++) {
    diff = samplePosOld_[n] - mean_;    
    var +=  (diff.ElementwiseProduct(diff));
  }
  var /= (double)(Nsamples_-start-1);
  return true;
}

double Condensation::GetMeanWeight() const
{
  double mean=0.0;
  unsigned start=(unsigned)rint(Nsamples_*(defaultInitFraction_+
                                           importanceFraction_));
  if (Nsamples_-start<=0){
    BCDOUT(D_COND_MEANVAR, "not enough samples "<<Nsamples_-start<<endl);
    return -1.0; 
  }
  Vector<double> diff;

  for (unsigned n=start;n<Nsamples_;n++) {
    //cout << setw(5) << n << setw(15) << sampleWeights_[n]<<endl;
    mean += sampleWeights_[n];
  }
  mean /= (double)(Nsamples_-start);
  return mean;
}