Condensation.hh

/* 
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
*/
#ifndef _CONDENSATION_GENERAL_HH_
#define _CONDENSATION_GENERAL_HH_
#include <Base/Common/BIASpragmaStart.hh>

#include <vector>
#include "Base/Debug/Debug.hh"
#include "Base/Math/Random.hh"
#include "Base/Math/Vector.hh"

//#define TIMING true
#ifdef TIMING
#include "Base/Debug/TimeMeasure.hh"
#endif

namespace BIAS {

  /** @class Condensation
   *  @ingroup g_stateestimation
      @brief CONDENSATION (CONditional DENSity EstimATION) is a state estimator
      very similiar to the Kalman Filter, not only for Gaussian-, but also for 
      any multi-modal distribution.
      Also it is a rather simple algorithm.

      The current state of a system, described by a state vector, is 
      estimated using a certain number of samples, which approximate the real
      densities. This is done by incorporating measurements ( p(Z|X) )
      with a prior distribution p(X)= p(X_t-1 | z_t).

      Usage:
      -# Write a class derived from Condensation and overload the 
      three necessary functions. See CondensImg and Examples/condensImgExample
      for examples.

      -# In your main program, just call Init() once with the desired 
      amount of samples
      -# optional, set parameter for the initial prior 
      distribution(write your own function) 

      -# call InitPrior(), which calls InitPriorPositions() and 
      precalculates the weigths etc. 

      In the loop: 

      -# call PredictNewSamplePositions();

      -# GetSamplePositions() and measure at those positions

      -# call your own necessary functions to give the condensation
      your observation values, e. g.SetObservation()

      -# call Process(), which calls EvaluateObservationDensities()
      and calculates the mean etc.

      -# Get the real state of the system, e.g. the mean with GetMean()
      @author Daniel Grest, Feb. 2004
  */

  class BIASStateEstimator_EXPORT Condensation : public Debug
  {
  public:
    Condensation();
    virtual ~Condensation();

    /** Init the Condensation directly after constructor, with the desired 
        amount of Samples, there more the better the approximation of 
        the real densities.
        The other Init functions are called here also. */      
    int Init(unsigned int nrSmaples);

    /** sets the sample positions for first time step */
    int InitPrior();

    /* This routine computes all of the new (unweighted) sample
       positions. For each sample, first a base is chosen, then the new
       sample position is computed by sampling from the prediction density
       p(x_t|x_t-1 = base) */
    int PredictNewSamplePositions();

    /** all samples are moved by this offset at the prediction step */
    void SetPredictionOffset(const BIAS::Vector<double> &offSet) 
    { offSet_=offSet; }

    inline const std::vector<BIAS::Vector<double> >& 
    GetSamplePositions() { return samplePosNew_;}

    /** This really does one iteration of Condensation. It calls 
        PredictSamplePosition() and EvaluateObservationDensity() 
        in the necessary order. */
    void Process();

    /** returns the mean state of the density distribution */
    Vector<double> GetMean() { return mean_;}

    /** returns the weighted variance as a scalar
        @author grest */
    double GetVariance() const;

    /** Returns the "weighted variance" of the sample positions in var.
        Initialization samples and importance samples are not used in 
        calculation.
        @author woelk 12/2004 */
    bool GetWeightedVariance(Vector<double>& var) const;

    /** Returns the variance of the sample positions in var. 
        Initialization samples and importance samples are not used in 
        calculation.
        @author woelk 12/2004 */
    bool GetVariance(Vector<double>& var) const;

    /** Returns the mean weight of all samples. Initialization samples and 
        importance samples are not used in calculation.
        @author woelk 12/2004 */
    double GetMeanWeight() const;

    /** Set the amount of samples, whose position is always taken
        by the prior distribution. 
        Default is 0.2. <br>
        fraction must be in [0..1]; */
    void SetDefaultInitFraction(double fraction); 

    /** Only specifiy a value unlike zero, if EvaluateImportanceWeights()
        is overloaded. */
    void SetImportanceSampleFraction(double fraction); 


    /** set a new processMean for the PredictSamplePosition step,
        see SecondOrderARP_()
    */
    inline void SetProcessMean(const BIAS::Vector<double> &newProcessMean)
    { processMean_=newProcessMean; }


    /** Here u can specify the model and process defaults */
    virtual void InitModelDefaults() = 0;

    /** The Prior condition is used to init the first sample positions.
        Calculates a initial pos for all samples.
        The prior cond. maybe defined by an image (if state vector is 2-dim) or
        by a mixture of gaussions or Uniform_distributed or ... or ...  */
    virtual void InitPriorPositions(unsigned int nrOfInitSamples) = 0;


    /** most important function, here u have to calculate a probability for each
        sample position based on your observations 
        and assign this prob. as a weight to the sample */
    virtual void EvaluateObservationDensities()=0;

    /** If you want to use importance sampling, then overload this function,
        which is the importance function. It should be a density distribution,
        which gives hint where to measure the object state.<br>
        Evaluate here the samplePosOld_, not the new ones <br>
        IMPORTANT: the scale of the importance weights must be equal to the
        normal weights, like in EvaluateObservationDensities() */
    virtual void EvaluateImportanceWeights() {};

  protected:
    bool _doSecondOrderARP; // should SecondOrderARP_ be used 
    /** question: real pos_tminus2 of each sample 
        or better old mean value for all samples? */
    inline void SecondOrderARP_(unsigned int indexOldSample, 
                                Vector<double> &newSample);

    /** The process model for a first-order auto-regressive process is:<br>
        x_{t+1} - mean = (x_t - mean)*scaling + sigma*w_t <br>
        where w_t is unit iid Gaussian noise, which is added by AddDiffusion 
    */
    //void FirstOrderARP_(unsigned int indexOldSample, 
    //                      Vector<double> &new_sample);

    /** This is binary search using cumulative probabilities to pick a base
        sample. The use of this routine makes Condensation O(NlogN) where N
        is the number of samples. It is probably better to pick base
        samples deterministically, since then the algorithm is O(N) and
        probably marginally more efficient, but this routine is kept here
        for conceptual simplicity and because it maps better to the
        published literature. */
    inline unsigned int PickOneSample_();


    inline unsigned int PickImportanceSample_();

    /** Once all the unweighted sample positions have been computed using
        predict_new_bases, this routine computes the weights by evaluating
        the observation density at each of the positions. Cumulative
        probabilities are also computed at the same time, to permit an
        efficient implementation of pick_base_sample using binary
        search. */
    virtual void CalculateBaseWeights_();

    /** Adds gaussian noise to the position, _can_ be overwritten if some
        other (i.e. non gaussian) noise is needed*/
    virtual void AddDiffusions_();

    /** adds offSet_ to the samplePosNew_, can be overwritten if some other 
        offset mechanism is needed */
    virtual void AddOffsets_();

    inline void CalculateMean_();

    unsigned int Nsamples_;
    unsigned int stateDim_;
    std::vector<Vector<double> > samplePosNew_; 
    std::vector<Vector<double> > samplePosOld_; 
    std::vector<Vector<double> > samplePosTminus2_;
    // indexTminus2_[i] is the index in samplePosTminus2_ for sample 
    // samplePosOld_[i]
    std::vector<unsigned int> indexTminus2_,oldIndices_;

    Vector<double> mean_, meanTminus1_;

    std::vector<double> sampleWeights_, cumulProbArray_;
    std::vector<double> sampleImportanceWeights_, cumulProbArrayImportance_;
    std::vector<double> correctionOfImportanceWeights_;

    double sumOfWeights_, sumOfImportanceWeights_;

    /** Process parameters */
    Vector<double> diffusionSigma_, processMean_;
    Vector<double> processFirstOrderScale_, processSecondOrderScale_;

    bool initial_;
    double defaultInitFraction_;
    double importanceFraction_;

    Random random_;
    double *_randomArray; // size Nsamples
    BIAS::Vector<double> offSet_;

    inline static int compdouble(const void*l, const void*r)
    { return (*((double*)l))>(*((double*)r)); }

  };


  inline unsigned int Condensation::PickOneSample_()
  {
    if (sumOfWeights_<1E-12) 
      return (int)random_.GetUniformDistributed(0, Nsamples_);

    double choice = random_.GetUniformDistributed(0.0, sumOfWeights_);

    register unsigned int low, middle, high;

    low = 0;
    high = Nsamples_-1;

    while (high>(low+1)) {
      middle = (high+low) >> 1;
      if (choice > cumulProbArray_[middle])
        low = middle;
      else high = middle;
    }

    if (cumulProbArray_[low]==cumulProbArray_[high])
      return low;
    else return high;
  }

  inline unsigned int Condensation::PickImportanceSample_()
  {
    double choice = random_.GetUniformDistributed(0.0,sumOfImportanceWeights_);

    unsigned int low, middle, high;

    low = 0;
    high = Nsamples_;

    while (high>(low+1)) {
      middle = (high+low)/2;
      if (choice > cumulProbArrayImportance_[middle])
        low = middle;
      else high = middle;
    }

    if (cumulProbArrayImportance_[low]==cumulProbArrayImportance_[high])
      return low;
    else return high;
  }

  inline void Condensation::SecondOrderARP_(unsigned int indexOldSample, 
                                            Vector<double> &newSample)
  {  
    // mean_ is in this moment the mean_ of the previous iteration
    for (unsigned int d=0;d<stateDim_;d++){
      BCDOUT(D_COND_DEBUG,"in SOARP d: "<<d<<std::endl);
      //    newSample[d]= processMean_[d] +
      //         (samplePosOld_[indexOldSample][d] - processMean_[d]) *
      //         processFirstOrderScale_[d] + 
      //         (mean_[d] - meanTminus1_[d]) *  processSecondOrderScale_[d];

      newSample[d]= processMean_[d] +
        (samplePosOld_[indexOldSample][d] - processMean_[d]) *
        processFirstOrderScale_[d] + 
        (samplePosOld_[indexOldSample][d]- 
         samplePosTminus2_[indexTminus2_[indexOldSample]][d]) *
        processSecondOrderScale_[d];        
    }
  }


  inline void Condensation::CalculateMean_()
  {
#ifdef TIMING
    TimeMeasure timer;
    timer.Start();
#endif

    if (sumOfWeights_<1E-20||sumOfWeights_!=sumOfWeights_) return; // new mean is old mean
    mean_.Set(0);
    for (unsigned int n=0;n<Nsamples_;n++) 
      mean_ += samplePosOld_[n] * sampleWeights_[n];
    mean_.MultiplyIP(1.0/sumOfWeights_);

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

}

#include <Base/Common/BIASpragmaEnd.hh>

#endif