normal.h

Go to the documentation of this file.

#ifndef GCOP_NORMAL_H
#define GCOP_NORMAL_H

#include <Eigen/Dense>
#include <vector>
#include "utils.h"

namespace gcop {
  
  using namespace Eigen;
  using namespace std;

  template <int _n = Dynamic>
  class Normal {
  public:
  typedef Matrix<double, _n, 1> Vectornd;
  typedef Matrix<double, _n, _n> Matrixnd;

  Normal(int n = 1);
  
  Normal(const Vectornd &mu, const Matrixnd &P);
  
  virtual ~Normal();
  
  double L(const Vectornd &x) const;
  
  
  double Sample(Vectornd &x);    
  
  bool Update();
  
  void Fit(const vector<pair<Vectornd, double> > xps, double a = 1);
  
  Vectornd mu;     
  Matrixnd P;      
  
  double det;      
  Matrixnd Pinv;   
  bool pd;         
  
  Matrixnd A;      
  Vectornd rn;     
  
  double norm;     
  
  int bd;          
  
  bool bounded;    
  Vectornd lb;     
  Vectornd ub;     
  
  LLT<Matrixnd> llt; 
  };
  


  template<int _n>
    Normal<_n>::Normal(int n):
    det(0),
    pd(false),
    norm(0),
    bd(0), 
    bounded(false) {
    
    if (_n == Dynamic) {
      mu.resize(n);
      P.resize(n,n);
      Pinv.resize(n,n);
      A.resize(n,n);
      rn.resize(n);
      lb.resize(n);
      ub.resize(n);
    }
    mu.setZero();
    P.setZero();
    Pinv.setZero();
    A.setZero();
    rn.setZero();
  }
  
  template<int _n>
    Normal<_n>::Normal(const Vectornd &mu, const Matrixnd &P):
    mu(mu),
    P(P),
    det(0),
    pd(false),
    norm(0),
    bd(0),
    bounded(false) {
    
    int n = mu.size();

    if (_n == Dynamic) {
      Pinv.resize(n,n);
      A.resize(n,n);
      rn.resize(n);
      lb.resize(n);
      ub.resize(n);
    }
    Pinv.setZero();
    A.setZero();
    rn.setZero();
    
    
    Update();
  }
  

  template<int _n>
    Normal<_n>::~Normal()
    {
    }


  template<int _n>
    double Normal<_n>::L(const Vectornd &x) const
    {
      if (!pd) {
        cout << "[W] Normal::L: not positive definite!" << endl;
      }
      
      Vectornd d = x - mu;
      return exp(-d.dot(Pinv*d))/2/norm;
    }
  
  template<int _n>
    bool Normal<_n>::Update()
    {
      llt.compute(P);
      
      if (llt.info() == Eigen::Success) {
        A = llt.matrixL();
        Pinv = P.inverse();
        det = P.determinant();
        norm = sqrt(pow(2*M_PI, mu.size())*det);    
        pd = true;
      } else {
        cout << "[W] Normal::Update: cholesky failed: P=" << P << endl;
        pd = false;    
      }
      
      return pd;
    }

  template<int _n>
    double Normal<_n>::Sample(Vectornd &x)
    {
      double p = 1;
      for (int i = 0; i < rn.size(); ++i) {
        rn(i) = random_normal();
        p *= rn(i);
      }
      
      x = mu + A*rn;
      
      if (bounded) {
        x = x.cwiseMax(lb);
        x = x.cwiseMin(ub);
      }

      return p;
    }
  
  template<int _n>
    void Normal<_n>::Fit(const vector<pair<Vectornd, double> > xws, double a)
    {
      int N = xws.size();
      
      Vectornd mu;
      if (_n == Dynamic)
        mu.resize(this->mu.size());

      mu.setZero();
      for (int j = 0; j < N; ++j) {
        const pair<Vectornd, double> &xw = xws[j];
        mu += xw.first*xw.second;
      }
      
      Matrixnd P;
      if (_n == Dynamic)
        P.resize(this->mu.size(), this->mu.size());
      P.setZero();

      for (int j = 0; j < N; ++j) {
        const pair<Vectornd, double> &xw = xws[j];
        Vectornd dx = xw.first - mu;
        if (!bd) {
          P += (xw.second*dx)*dx.transpose();
        } else {
          int b = dx.size()/bd;
          for (int i = 0; i < b; ++i) {
            int bi = i*bd;
            Vectornd bdx = dx.segment(bi, bd);        
            P.block(bi, bi, bd, bd) += (xw.second*bdx)*bdx.transpose();       
          }
        }
      }
      
      if (fabs(a-1) < 1e-16) {
        this->mu = mu;
        this->P = P;
      } else {
        this->mu = a*mu + (1-a)*this->mu;
        this->P = a*P + (1-a)*this->P;
      }
    }  
}


#endif