gp.hpp 4.38 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
#ifndef GP_HPP_
#define GP_HPP_

#include <cassert>
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Cholesky>


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

namespace limbo {
  namespace model {
    template<typename Params, typename KernelFunction, typename MeanFunction>
    class GP {
     public:
      GP() : _dim(-1) {}
      // useful because the model might created  before having samples
      GP(int d) : _dim(d), _kernel_function(d) {}

      void compute(const std::vector<Eigen::VectorXd>& samples,
                   const std::vector<double>& observations,
                   double noise) {
        if (_dim == -1) {
          assert(samples.size() != 0);
          assert(observations.size() != 0);
          assert(samples.size() == observations.size());
          _dim = samples[0].size();
        }
        _samples = samples;
        _observations.resize(observations.size());
        _noise = noise;

Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
35
        for (int i = 0; i < _observations.size(); ++i)
36
          _observations(i) = observations[i];
37
38

        _mean_observation = _observations.sum() / _observations.size();
39
40
41

        _mean_vector.resize(_samples.size());
        for (int i = 0; i < _mean_vector.size(); i++)
42
          _mean_vector(i) = _mean_function(_samples[i], *this);
43
44
45
46
47
48
49
50
        _obs_mean = _observations - _mean_vector;

        _compute_kernel();
      }

      // return mu, sigma
      std::tuple<double, double> query(const Eigen::VectorXd& v) const {
        if (_samples.size() == 0)
51
52
          return std::make_tuple(_mean_function(v, *this),
                                 sqrt(_kernel_function(v, v)));
53
54
55
56
57
58
59

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

      double mu(const Eigen::VectorXd& v) const {
        if (_samples.size() == 0)
60
          return _mean_function(v, *this);
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
        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;
      }
Jean-Baptiste Mouret's avatar
misc    
Jean-Baptiste Mouret committed
78
79
80
      double max_observation() const {
        return _observations.maxCoeff();
      }
81
82
      double mean_observation() const {
        return _mean_observation;
83
      }
84
85
86
87
88
89
90
91
92
93
94
     protected:
      int _dim;
      KernelFunction _kernel_function;
      MeanFunction _mean_function;

      std::vector<Eigen::VectorXd> _samples;
      Eigen::VectorXd _observations;
      Eigen::VectorXd _mean_vector;
      Eigen::VectorXd _obs_mean;
      double _noise;
      Eigen::VectorXd _alpha;
95
      double _mean_observation;
96
97
98
99
100
101
102
103
104

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

      void _compute_kernel() {
        // O(n^2) [should be negligible]
        _kernel.resize(_observations.size(), _observations.size());
Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
105
106
        for (int i = 0; i < _observations.size(); i++)
          for (int j = 0; j < _observations.size(); ++j)
107
108
109
110
111
112
113
114
115
116
117
118
119
            _kernel(i, j) = _kernel_function(_samples[i], _samples[j]) + _noise;

        // O(n^3)
        _inverted_kernel = _kernel.inverse();

        _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);
      }

      double _mu(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
120
        return _mean_function(v, *this) + k.transpose() * _alpha;
Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
121
122
        //        return _mean_function(v)
        //               + (k.transpose() * _inverted_kernel * (_obs_mean))[0];
123
124
125
126
      }
      double _sigma(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
        Eigen::VectorXd z = _llt.matrixL().solve(k);
        return  _kernel_function(v, v) - z.dot(z);
Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
127
        //        return  _kernel_function(v, v) - (k.transpose() * _inverted_kernel * k)[0];
128
129
130
131
132
133
134
135
136
137
138
      }
      Eigen::VectorXd _compute_k(const Eigen::VectorXd& v) const {
        Eigen::VectorXd k(_samples.size());
        for (int i = 0; i < k.size(); i++)
          k[i] = _kernel_function(_samples[i], v);
        return k;
      }
    };
  }
}
#endif