UnscentedTransform.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 <Base/Common/W32Compat.hh>
#include <MathAlgo/UnscentedTransform.hh>
#include <MathAlgo/SVD.hh>
#include <MathAlgo/Lapack.hh>
#include <Base/Common/CompareFloatingPoint.hh>
#include <Base/Debug/Exception.hh>
#include <Base/Math/Matrix.hh>

using namespace BIAS;
using namespace std;

UnscentedTransform::
UnscentedTransform()
  : UncertaintyTransformBase(), Alpha_(0.001), Beta_(2.0), Kappa_(0.0),
    UseSVD_(true)
{
  NewDebugLevel("D_UT_SIGMA_POINTS");
  NewDebugLevel("D_UT_TRANSFORM");
  NewDebugLevel("D_UT_PARAM");
}


UnscentedTransform::
~UnscentedTransform()
{}


int UnscentedTransform::
Transform(const Vector<double>& src_mean, const Matrix<double>& src_cov,
          Vector<double>& dst_mean, Matrix<double>& dst_cov) const
{
  //  BIASABORT;

  if (src_cov.Trace()<numeric_limits<double>::epsilon()){
    //    BIASWARN("Zero covariance matrix detected.");
    if (Transform_(src_mean, dst_mean) <0) {
        BCDOUT(D_UT_TRANSFORM, "transformation of mean failed, returning -1");
        return -1;
    }
    const int dim = dst_mean.Size();
    dst_cov.newsize(dim, dim);
    dst_cov.SetZero();
  } else {
    vector<WeightedSigmaPoint> sigma_points;
   
    // check result dimension
    int res = 0;
    if ((res = Transform_(src_mean, dst_mean))!=0){
      BCDOUT(D_UT_TRANSFORM, "transformation of mean failed, returning -1");
      return -1;
    } else {
      BCDOUT(D_UT_TRANSFORM, "transformation of mean yielded "<<dst_mean);
    }
#ifdef BIAS_DEBUG
    // transform returned 0, so results MUST NOT contain any invalid numbers!
    for (int d=0; d<dst_mean.size(); d++) {
      // NaN ?
      INFNANCHECK(dst_mean[d]);
    }
#endif

    // compute the sigma points
    if (ComputeSigmaPoints_(src_mean, src_cov, sigma_points)!=0) return 1;

    const int dim = dst_mean.Size();
    // initialize result
    dst_mean.SetZero();
    dst_cov.newsize(dim, dim);
    dst_cov.SetZero();

    Vector<double> diff, src;
    vector<WeightedSigmaPoint>::iterator it;
    for (it=sigma_points.begin(); it!=sigma_points.end(); it++){
      // transform the sigma points
      src = it->point;
      BCDOUT(D_UT_TRANSFORM, "src: "<<src<<endl);
      if ((res = Transform_(src, it->point))!=0){ return 1; }
#ifdef BIAS_DEBUG
      // transform returned 0, so results MUST NOT contain any invalid numbers!
      for (int d=0; d<dst_mean.size(); d++) {
        INFNANCHECK(it->point[d]);
      }
#endif

      BCDOUT(D_UT_TRANSFORM, "dst: "<<it->point<<endl);
      // compute weighted mean
      dst_mean += (it->point * it->weight_mean);
      BCDOUT(D_UT_TRANSFORM, "mean: "<<dst_mean<<endl);
    }

    for (it=sigma_points.begin(); it!=sigma_points.end(); it++){
      // compute the covariance
      diff = it->point - dst_mean;
      BCDOUT(D_UT_TRANSFORM, "diff: "<<diff<<endl);
      BCDOUT(D_UT_TRANSFORM, "weight "<< it->weight_cov
                << " * outer product: " <<  diff.OuterProduct(diff) << endl);
      dst_cov += it->weight_cov * diff.OuterProduct(diff);
      BCDOUT(D_UT_TRANSFORM, "cov: "<<dst_cov<<endl);
    }
    // now we check finally whether cov is valid
    for (int row=0; row<dst_cov.num_rows(); row++) {
      for (int col=0; col<dst_cov.num_cols(); col++) {
        if (BIAS_ISNAN(dst_cov[row][col]) || BIAS_ISINF(dst_cov[row][col])) {
          // though each individual point was good, cov could not be computed !
          return 1;
        }
      }
    }
  }
  return 0;
}


int UnscentedTransform::
ComputeSigmaPoints_(const Vector<double>& src_mean,
                    const Matrix<double>& src_cov,
                    std::vector<WeightedSigmaPoint>& sigma_points) const
{
  const int dim = src_mean.Size();
  const double dbl_dim = static_cast<double>(dim);
  const double alpha_square = Alpha_ * Alpha_;
  const double lambda = alpha_square * ( dbl_dim + Kappa_ ) - dbl_dim;
  //const double dim_plus_lambda = dbl_dim + lambda;
  const double dim_plus_lambda = alpha_square * ( dbl_dim + Kappa_ );
  BIASASSERT(fabs(dim_plus_lambda)>DBL_EPSILON);
  //cout << "dim_plus_lambda="<<dim_plus_lambda<<endl;
  BCDOUT(D_UT_PARAM, "Alpha: "<<Alpha_<<"  Beta : "<<Beta_
            <<"  Kappa: "<<Kappa_<<endl);
  sigma_points.resize(2 * dim + 1);


#ifdef BIAS_DEBUG
  // check if input matrix contains junk
  for (int r=0; r<src_cov.num_rows(); r++) {
    INFNANCHECK(src_mean[r]);
    for (int c=0; c<src_cov.num_cols(); c++) {
      INFNANCHECK(src_cov[r][c]);
    }
  }
#endif



  SVD svd;
  Matrix<double> sqrt_cov;

  // numerically very unstable without normalization !!!
#define UT_NORMALIZE
#ifdef UT_NORMALIZE
  double factor = 0.0;
  for (int i=0; i<dim; i++) {
#ifdef BIAS_DEBUG
    // covariance matrix cannot have negative diagonal elements
    // this problem cannot be solved here, but must be solved in the
    // calling class !
  //  BIASASSERT(src_cov[i][i]>=0.0);

#endif
    factor += src_cov[i][i];
  }

  if (Less(fabs(factor), numeric_limits<double>::epsilon())){
    BEXCEPTION("cannot normalize zero covariance matrix "<<src_cov);
  }

  Matrix<double> normalized_cov = src_cov / factor;
  if (UseSVD_) {
    sqrt_cov = svd.SqrtT(normalized_cov);
    //cout << "cov: "<<normalized_cov<<"\n sqrt * sqrt = "
    // <<sqrt_cov*sqrt_cov.Transpose()<<endl;
  } else {
    // enforce positive definiteness by adding epsilon to diagonal elements
    const int num=normalized_cov.GetCols();
    const double eps_fac = 2.0* numeric_limits<double>::epsilon();
    for (int i=0; i<num; i++)
      normalized_cov[i][i] += eps_fac;
    if (Lapack_Cholesky_SymmetricPositiveDefinit(normalized_cov,
                                                 sqrt_cov, 'L')!=0){
#ifdef BIAS_DEBUG
        BIASERR("error computing matrix sqrt for "<<normalized_cov<<endl);
#endif
        return -1;
    }
    //cout << "cov: "<<normalized_cov<<"\n sqrt * sqrt = "
    // <<sqrt_cov*sqrt_cov.Transpose()<<endl;
  }
  factor *= dim_plus_lambda;
  factor = sqrt(factor);
#else
  if (UseSVD_){
    sqrt_cov = svd.SqrtT(dim_plus_lambda * src_cov);
  } else {
    Matrix<double> tmp = dim_plus_lambda * src_cov;
    // enforce positive definiteness by adding epsilon to diagonal elements
    const int num=tmp.GetCols();
    const double eps_fac = 2.0* numeric_limits<double>::epsilon();
    for (int i=0; i<num; i++)
      tmp[i][i] += eps_fac;
    if (Lapack_Cholesky_SymmetricPositiveDefinit(tmp, sqrt_cov, 'L')!=0){
#ifdef BIAS_DEBUG
      BIASERR("error computing matrix sqrt for "<<tmp<<endl);
#endif
      return -1;
    }
  }
#endif

  if (sqrt_cov.num_cols()==0) {
#ifdef BIAS_DEBUG
    BIASERR("could not compute square root of cov "<< src_cov);
#endif
    return -1;
    //    sqrt_cov = src_cov;
    //    sqrt_cov.SetZero();
    //    //BIASASSERT(sqrt_cov.num_cols()!=10);
    //    for (unsigned int i=0; (int)i<sqrt_cov.num_cols(); i++) {
    //      sqrt_cov[i][i] = 1e-10;
    //    }
  }

  BIASASSERT(sqrt_cov.num_cols() == src_cov.num_cols());

#ifdef BIAS_DEBUG
  // check if factorization succeeded or whether we have junk data in matrix
  for (int r=0; r<sqrt_cov.num_rows(); r++) {
    for (int c=0; c<sqrt_cov.num_cols(); c++) {
      INFNANCHECK(sqrt_cov[r][c]);
    }
  }
#endif

  Vector<double> vec;

  sigma_points[0].point = src_mean;
  sigma_points[0].weight_mean = lambda / dim_plus_lambda;
  sigma_points[0].weight_cov =
    lambda / dim_plus_lambda + 1.0 - alpha_square + Beta_;
#ifdef BIAS_DEBUG
  INFNANCHECK(sigma_points[0].weight_mean);
  INFNANCHECK(sigma_points[0].weight_cov);
  for (int ii=0; ii<sigma_points[0].point.size(); ii++){
    INFNANCHECK(sigma_points[0].point[ii]);
  }
#endif
  int ind;
  for (int i=0; i<dim; i++){
    ind = i+1;

    vec = sqrt_cov.GetCol(i);
#ifdef UT_NORMALIZE
    vec *= factor;
#endif
    sigma_points[ind].point = src_mean + vec;
    sigma_points[ind+dim].point = src_mean - vec;
    sigma_points[ind].weight_mean =
      sigma_points[ind+dim].weight_mean =
      sigma_points[ind].weight_cov =
      sigma_points[ind+dim].weight_cov =
      1.0 / ( 2.0 * dim_plus_lambda );
#ifdef BIAS_DEBUG
    INFNANCHECK(sigma_points[ind].weight_mean);
    INFNANCHECK(sigma_points[ind].weight_cov);
    for (int ii=0; ii<sigma_points[ind].point.size(); ii++){
      INFNANCHECK(sigma_points[ind].point[ii]);
      INFNANCHECK(sigma_points[ind+dim].point[ii]);
    }
#endif
  }
#ifdef BIAS_DEBUG
  if (DebugLevelIsSet("D_UT_SIGMA_POINTS")){
    for (int k=0; k<2; k++){
      cout << "sp["<<k<<"] = "<<sigma_points[k].point<<" W_m = "
           <<sigma_points[k].weight_mean << endl<<" W_c = "
           <<sigma_points[k].weight_cov << endl;
    }
  }

#endif
  return 0;
}