UnscentedTransform.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 _UnscentedTransform_hh_
#define _UnscentedTransform_hh_

#include <MathAlgo/UncertaintyTransformBase.hh>
#include <Base/Math/Vector.hh>

#include <vector>

namespace BIAS {

  // forward declaration
  template <class T> class BIASMathBase_EXPORT Matrix;

  struct BIASMathAlgo_EXPORT WeightedSigmaPoint 
  {
    Vector<double> point;
    double weight_mean;
    double weight_cov;
  };

  /** @class UnscentedTransform
   * @ingroup g_mathalgo
      @brief uses the unscented transformation to map a normal distribututed 
      random variable using a nonlinear transformation

      See S.J.Julier and J.K.Uhlmann "A New Extension of the Kalman Filter to 
      Nonlinear Systems" for a detailed description of the unscented transform.
      The unscented transform approximates the transformation of the covariance
      matrix up to second order of the according Taylor series.
 
      To use this class derive from it and implement the non linear 
      transformation function Transform_().

      @author woelk 01/2006 (tested) */
  class BIASMathAlgo_EXPORT UnscentedTransform 
    : public UncertaintyTransformBase
  {
  public:
    UnscentedTransform();

    virtual ~UnscentedTransform();

    /** @brief computes the second order approximation of the 
        transformations of the mean and the associated covariance for the 
        point transformation Transform_().
        @returns 0 on success 
                -1 when transformation of mean failed
                +1 when transfromation of sigma point failed */ 
    int Transform(const Vector<double>& src_mean, 
                  const Matrix<double>& src_cov,
                  Vector<double>& dst_mean, Matrix<double>& dst_cov) const;

    /** Sets the alpha parameter. 
        The alpha parameter determines the spread of the sigma points */
    inline void SetAlpha(const double alpha)
    { Alpha_ = alpha; }

    /** Sets the beta parameter.
        beta is used to incorporate prior knowledge of the distribution of x. 
        For Gaussian distributions beta = 2.0 is optiomal */
    inline void SetBeta(const double beta)
    { Beta_ = beta; }

    inline void SetUseSVD(const bool use_svd)
    { UseSVD_ = use_svd; }

  protected:
    /** the alpha parameter determines the spread of the sigma points */
    double Alpha_;
    /** beta is used to incorporate prior knowledge of the distribution of x. 
        For Gaussian distributions beta = 2.0 is optiomal */
    double Beta_;
    /** kappa is a secondary scaling parameter. In other papers from Uhlman 
        kappa is fixed to 1.0 */
    double Kappa_; 
    
    bool UseSVD_;

    /** The covariance of a n-dimensional vector is approximated using 2n+1 
        so called sigma points with associated weights. The points and weights
        are computed in this function 
        @return 0=ok, <0 on error (bad cov, ...)
    */
    int ComputeSigmaPoints_(const Vector<double>& src_mean, 
                            const Matrix<double>& src_cov,
                            std::vector<WeightedSigmaPoint>& sigma_points) 
      const;

    /** @brief implements a point transformation
        @returns 0 on success */
    virtual int Transform_(const Vector<double>& src,
                           Vector<double>& dst) const = 0;
    
  }; // class

} // namespace

#endif // _UnscentedTransform_hh_