gp.hpp 4.89 KB
Newer Older
1

2
3
4
5
6
7
8
#ifndef GP_HPP_
#define GP_HPP_

#include <cassert>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Cholesky>
9
#include <limits>
10
11
12
13
14
15

#include "kernel_functions.hpp"
#include "mean_functions.hpp"

namespace limbo {
  namespace model {
16
17
18


    template<typename Params, typename KernelFunction, typename MeanFunction, typename ObsType= Eigen::VectorXd>
19
20
21
    class GP {
     public:
      GP() : _dim(-1) {}
22
      // useful because the model might be created  before having samples
23
24
25
      GP(int d) : _dim(d), _kernel_function(d) {}

      void compute(const std::vector<Eigen::VectorXd>& samples,
26
                   const std::vector<ObsType>& observations,
27
                   double noise) {
28

29

30
31
32
33
34
35
        if (_dim == -1) {
          assert(samples.size() != 0);
          assert(observations.size() != 0);
          assert(samples.size() == observations.size());
          _dim = samples[0].size();
        }
36

37
        _samples = samples;
38
        _observations.resize(observations.size(),observations[0].size());
39
40
        _noise = noise;

41
	int obs_dim=observations[0].size();
42

43
44
45
46
47
48
49
50
        for (int i = 0; i < _observations.rows(); ++i) 
          _observations.row(i) = observations[i];
	_mean_observation.resize(obs_dim);
	for(int i=0; i< _observations.cols(); i++)
	  _mean_observation(i) = _observations.col(i).sum() / _observations.rows();

        _mean_vector.resize(_samples.size(),obs_dim);
        for (int i = 0; i < _mean_vector.rows(); i++)
51
	  _mean_vector.row(i) = _mean_function(_samples[i], *this); 
52
53
        _obs_mean = _observations - _mean_vector;

54

55
        _compute_kernel();
56

57
58
      }

59
      // return mu, sigma (unormaliz)
60
      std::tuple<ObsType, double> query(const Eigen::VectorXd& v) const {
61
        if (_samples.size() == 0)
62
63
          return std::make_tuple(_mean_function(v, *this),
                                 sqrt(_kernel_function(v, v)));
64
65
66
67
68

        Eigen::VectorXd k = _compute_k(v);
        return std::make_tuple(_mu(v, k), _sigma(v, k));
      }

69
      ObsType mu(const Eigen::VectorXd& v) const {
70
        if (_samples.size() == 0)
71
          return _mean_function(v, *this);
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
        return _mu(v, _compute_k(v));
      }
      double sigma(const Eigen::VectorXd& v) const {
        if (_samples.size() == 0)
          return sqrt(_kernel_function(v, v));
        return _sigma(v, _compute_k(v));
      }
      int dim() const {
        assert(_dim != -1);//need to compute first !
        return _dim;
      }
      const KernelFunction& kernel_function() const {
        return _kernel_function;
      }
      const MeanFunction& mean_function() const {
        return _mean_function;
      }
89
90
91
      ObsType max_observation() const {  
	if(_observations.cols()>1)
	  std::cout<<"WARNING max_observation with multi dimensional observations doesn't make sense"<<std::endl;
Jean-Baptiste Mouret's avatar
misc    
Jean-Baptiste Mouret committed
92
93
        return _observations.maxCoeff();
      }
94
      ObsType mean_observation() const {
95
        return _mean_observation;
96
      }
97
98

      const Eigen::MatrixXd& mean_vector() const{return _mean_vector;}
99
100
101
102
103
104
     protected:
      int _dim;
      KernelFunction _kernel_function;
      MeanFunction _mean_function;

      std::vector<Eigen::VectorXd> _samples;
105
106
107
      Eigen::MatrixXd _observations;
      Eigen::MatrixXd _mean_vector;
      Eigen::MatrixXd _obs_mean;
108

109
      double _noise;
110
111
      Eigen::MatrixXd _alpha;
      ObsType _mean_observation;
112
113
114
115
116
117
118

      Eigen::MatrixXd _kernel;
      Eigen::MatrixXd _inverted_kernel;
      Eigen::MatrixXd _l_matrix;
      Eigen::LLT<Eigen::MatrixXd> _llt;

      void _compute_kernel() {
119

120
        // O(n^2) [should be negligible]
121
122
123
        _kernel.resize(_samples.size(), _samples.size());
        for (size_t i = 0; i < _samples.size(); i++)
          for (size_t j = 0; j < _samples.size(); ++j)
124
125
            _kernel(i, j) = _kernel_function(_samples[i], _samples[j]) + ((i==j)?_noise:0);

126
127

        // O(n^3)
128
        //  _inverted_kernel = _kernel.inverse();
129
130
131
132
133
134

        _llt = Eigen::LLT<Eigen::MatrixXd>(this->_kernel);

        // alpha = K^{-1} * this->_obs_mean;
        _alpha = _llt.matrixL().solve(this->_obs_mean);
        _llt.matrixL().adjoint().solveInPlace(_alpha);
135

136
137
      }

138
      ObsType _mu(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
139
140
        return  (k.transpose() * _alpha) + _mean_function(v, *this).transpose();

Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
141
142
        //        return _mean_function(v)
        //               + (k.transpose() * _inverted_kernel * (_obs_mean))[0];
143
144
145
      }
      double _sigma(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
        Eigen::VectorXd z = _llt.matrixL().solve(k);
146
147
148
	double res= _kernel_function(v, v) - z.dot(z);
	return (res<=std::numeric_limits<double>::epsilon())?0:res;

Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
149
        //        return  _kernel_function(v, v) - (k.transpose() * _inverted_kernel * k)[0];
150
151
      }
      Eigen::VectorXd _compute_k(const Eigen::VectorXd& v) const {
152

153
154
155
156
157
158
159
160
161
        Eigen::VectorXd k(_samples.size());
        for (int i = 0; i < k.size(); i++)
          k[i] = _kernel_function(_samples[i], v);
        return k;
      }
    };
  }
}
#endif