ce.h

Go to the documentation of this file.

// This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP)
//
// Copyright (C) 2004-2014 Marin Kobilarov <marin(at)jhu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef GCOP_CE_H
#define GCOP_CE_H

#include <vector>
#include "gmm.h"
#include <limits>

namespace gcop {
  
  using namespace Eigen;
  using namespace std;
  
  template <int _n = Dynamic> 
    class Ce {
  public:  
  typedef Matrix<double, _n, 1> Vectornd;
  typedef Matrix<double, _n, _n> Matrixnd;
  
  Ce(int n = _n, int k = 1, const Matrixnd *S = 0);
  
  virtual ~Ce();
  
  void Reset();
  
  virtual void AddSample(const Vectornd &z, double c);
  
  void Select();
  
  bool Fit();
  
  virtual double Sample(Vectornd &z);
  
  const Vectornd& Best();
  
  int n;          
      
  Gmm<_n> gmm;    

  Matrixnd S;     

  vector<pair<Vectornd, double> > zps;  

  vector<double> cs;    

  double rho;     

  double alpha;   

  bool mras;      

  double b;       

  bool bAuto;    

  bool inc;      

  Vectornd zmin; 

  double Jmin;    

  double Jmax;    

  double Jave;    

  };
  
  template <int _n>
    Ce<_n>::Ce(int n,
               int k,
               const Matrixnd *S) :
    n(n),
    gmm(n, k),
    rho(.1),
    alpha(.9),
    mras(false),
    b(1),
    bAuto(true),
    inc(false),
    Jmin(std::numeric_limits<double>::max()),
    Jmax(0),
    Jave(std::numeric_limits<double>::max())
      {
        if (_n == Dynamic) {
          this->S.resize(n, n);
          this->zmin.resize(n);
        } else {
          assert(_n == n);
        }
        
        if (S)
          this->S = *S;
        else
          this->S.setZero();
      }
  
  template <int _n>
    Ce<_n>::~Ce()
    {
    }


  template <int _n>
    void Ce<_n>::Reset()
    {
      zps.clear();
      cs.clear();
      Jmin = std::numeric_limits<double>::max();
      Jmax = 0;
    }

  template <int _n>
    void Ce<_n>::AddSample(const Vectornd &z, double c) 
    { 
      int Ns = zps.size();
      if (!Ns) {
        Jave = c;
        Jmax = c;
      } else {
        Jave = (Jave*Ns + c)/(Ns + 1);
      }
      zps.push_back(make_pair(z, c));
      cs.push_back(c);

      if (Jmin > c) {
        zmin = z;
        Jmin = c;
      }

      if (Jmax < c) {
        Jmax = c;
      }      
    }

  template <int _n>
    bool zpSort(const pair<Matrix<double, _n, 1>, double> &zpa, const pair<Matrix<double, _n, 1>, double> &zpb)
    {
      return zpa.second < zpb.second;
    }

  template <int _n>
    void Ce<_n>::Select()
    {
      if (mras) {
        // set b to minimum cost
        if (bAuto) {
          b = Jmin;
          /*
          b = std::numeric_limits<double>::max();
          for (int j = 0; j < cs.size(); ++j) {
            if (b > cs[j])
              b = cs[j];
          }
          */
        }
        return;
      }
  
      int N = zps.size();
      int Ne = MIN((int)ceil(N*rho), N);
  
      if (Ne < N) {
        std::sort(zps.begin(), zps.end(), zpSort<_n>);
        std::sort(cs.begin(), cs.end());   // this is redundant but is kept for consistency
        zps.resize(Ne);
        cs.resize(Ne);
      }
    }


  template <int _n>
    bool Ce<_n>::Fit()
    {       
      int N = zps.size();
  
      if (mras) {
        double cn = 0;
        for (int j = 0; j < N; ++j) {
          pair<Vectornd, double> &zp = zps[j];
          zp.second = exp(-b*cs[j]);     // pdf
          cn += zp.second;               // normalizer
        }
        assert(cn > 0);
        for (int j = 0; j < N; ++j) 
          zps[j].second /= cn;               // normalize
    
        gmm.Fit(zps, alpha, 50, &S);
      } else {
        for (int j = 0; j < N; ++j) 
          zps[j].second = 1.0/N;             // probability
    
        gmm.Fit(zps, alpha, 50, &S);
      }

      return gmm.Update();
    }

  template <int _n>
    double Ce<_n>::Sample(Vectornd &z) 
    {
      return gmm.Sample(z);
    }

  template <int _n>
    const Matrix<double, _n, 1>& Ce<_n>::Best() 
    {
      return zmin;
    }
}


#endif