gp.hpp 8.01 KB
Newer Older
1
2
3
4
5
6
7
#ifndef GP_HPP_
#define GP_HPP_

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

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

namespace limbo {
  namespace model {
15
    template<typename Params, typename KernelFunction, typename MeanFunction, typename ObsType= Eigen::VectorXd>
16
17
    class GP {
     public:
Aneoshun's avatar
Aneoshun committed
18
      GP() : _dim_in(-1), _dim_out(-1) {}
19
      // useful because the model might be created  before having samples
Aneoshun's avatar
Aneoshun committed
20
      GP(int dim_in, int dim_out) : _dim_in(dim_in),_dim_out(dim_out), _kernel_function(dim_in), _mean_function(dim_out) {}
21
22

      void compute(const std::vector<Eigen::VectorXd>& samples,
23
                   const std::vector<ObsType>& observations,
24
                   double noise, const std::vector<Eigen::VectorXd>& bl_samples = std::vector<Eigen::VectorXd>()) {
Aneoshun's avatar
Aneoshun committed
25
        if (_dim_in == -1) {
26
27
28
          assert(samples.size() != 0);
          assert(observations.size() != 0);
          assert(samples.size() == observations.size());
Aneoshun's avatar
Aneoshun committed
29
          _dim_in = samples[0].size();
30
          _dim_out= observations[0].size();
31
        }
32

33
34
        _samples = samples;

35
        _observations.resize(observations.size(),observations[0].size());
36
37
38
        for (int i = 0; i < _observations.rows(); ++i) 
          _observations.row(i) = observations[i];

39
40
41
        _mean_observation.resize(_dim_out);
        for(int i=0; i< _observations.cols(); i++)
          _mean_observation(i) = _observations.col(i).sum() / _observations.rows();
42

43
44
45
46
47
48
        _noise = noise;

        _bl_samples = bl_samples;

        _compute_obs_mean();
        _compute_kernel();
49
50
      }

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

56
        if (_samples.size() == 0)
57
          return std::make_tuple(_mean_function(v, *this), _sigma(v, _compute_k_bl(v, _compute_k(v))));
58
59

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

63
      ObsType mu(const Eigen::VectorXd& v) const {
64
        if (_samples.size() == 0)
65
          return _mean_function(v, *this);
66
67
        return _mu(v, _compute_k(v));
      }
68

69
      double sigma(const Eigen::VectorXd& v) const {
70
        if (_samples.size() == 0 && _bl_samples.size() == 0)
71
          return sqrt(_kernel_function(v, v));
72
        return _sigma(v, _compute_k_bl(v, _compute_k(v)));
73
      }
74

Aneoshun's avatar
Aneoshun committed
75
76
77
78
      int dim_in() const {
        assert(_dim_in != -1);//need to compute first !
        return _dim_in;
      }
79

Aneoshun's avatar
Aneoshun committed
80
81
82
      int dim_out() const {
        assert(_dim_out != -1);//need to compute first !
        return _dim_out;
83
      }
84

85
86
87
      const KernelFunction& kernel_function() const {
        return _kernel_function;
      }
88

89
90
91
      const MeanFunction& mean_function() const {
        return _mean_function;
      }
92
93
94
95
96

      ObsType max_observation() const {
      	 if(_observations.cols()>1)
	 std::cout<<"WARNING max_observation with multi dim_inensional observations doesn't make sense"<<std::endl;
       return _observations.maxCoeff();
Jean-Baptiste Mouret's avatar
misc    
Jean-Baptiste Mouret committed
97
      }
98

99
      ObsType mean_observation() const {
100
        return _samples.size() > 0 ? _mean_observation : Eigen::VectorXd::Zero(_dim_in);
101
      }
102

103
104
105
106
      const Eigen::MatrixXd& mean_vector() const {
       return _mean_vector; 
     }

107
108
109
110
     int nb_samples() const {return _samples.size();}

     int nb_bl_samples() const {return _bl_samples.size();}

111
     protected:
Aneoshun's avatar
Aneoshun committed
112
113
      int _dim_in;
      int _dim_out;
Aneoshun's avatar
Aneoshun committed
114

115
116
117
118
      KernelFunction _kernel_function;
      MeanFunction _mean_function;

      std::vector<Eigen::VectorXd> _samples;
119
      Eigen::MatrixXd _observations;
120
      std::vector<Eigen::VectorXd> _bl_samples;// black listed samples
121
122
      Eigen::MatrixXd _mean_vector;
      Eigen::MatrixXd _obs_mean;
123

124
      double _noise;
125
126
      Eigen::MatrixXd _alpha;
      ObsType _mean_observation;
127
128

      Eigen::MatrixXd _kernel;
129
      // Eigen::MatrixXd _inverted_kernel;
130
      Eigen::MatrixXd _l_matrix;
131
132
      Eigen::LLT<Eigen::MatrixXd> _llt;      
      Eigen::MatrixXd _inv_bl_kernel;
133

134
135
      void _compute_obs_mean() {
        _mean_vector.resize(_samples.size(), _dim_out);
Aneoshun's avatar
Aneoshun committed
136
        for (int i = 0; i < _mean_vector.rows(); i++)
137
          _mean_vector.row(i) = _mean_function(_samples[i], *this); 
Aneoshun's avatar
Aneoshun committed
138
139
140
        _obs_mean = _observations - _mean_vector;
      }

141
142
      void _compute_kernel() {
        // O(n^2) [should be negligible]
143
144
145
        _kernel.resize(_samples.size(), _samples.size());
        for (int i = 0; i < _samples.size(); i++)
          for (int j = 0; j < _samples.size(); ++j)
146
            _kernel(i, j) = _kernel_function(_samples[i], _samples[j])
Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
147
                            +  ( (i == j) ? _noise : 0); // noise only on the diagonal
148
149

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

152
        _llt = Eigen::LLT<Eigen::MatrixXd>(_kernel);
153
154

        // alpha = K^{-1} * this->_obs_mean;
155
        _alpha = _llt.matrixL().solve(_obs_mean);
156
        _llt.matrixL().adjoint().solveInPlace(_alpha);
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
        if(_bl_samples.size() == 0)
          return;        

        Eigen::MatrixXd A1 = Eigen::MatrixXd::Identity(this->_samples.size(), this->_samples.size());
        _llt.matrixL().solveInPlace(A1);
        _llt.matrixL().transpose().solveInPlace(A1);
        _inv_bl_kernel.resize(_samples.size() + _bl_samples.size(), _samples.size() + _bl_samples.size());

        Eigen::MatrixXd B(_samples.size(), _bl_samples.size());
        for (size_t i = 0; i < _samples.size(); i++)
          for (size_t j = 0; j < _bl_samples.size(); ++j)
            B(i,j) = _kernel_function(_samples[i], _bl_samples[j]);

        Eigen::MatrixXd D(_bl_samples.size(),_bl_samples.size());
        for (size_t i = 0; i < _bl_samples.size(); i++)
          for (size_t j = 0; j < _bl_samples.size(); ++j)
            D(i,j) = _kernel_function(_bl_samples[i], _bl_samples[j]) + ((i==j) ? _noise : 0);

        Eigen::MatrixXd comA = (D - B.transpose() * A1 * B);
        Eigen::LLT<Eigen::MatrixXd> llt_bl(comA);
        Eigen::MatrixXd comA1 = Eigen::MatrixXd::Identity(_bl_samples.size(), _bl_samples.size());
        llt_bl.matrixL().solveInPlace(comA1);
        llt_bl.matrixL().transpose().solveInPlace(comA1);

        //fill the matrix block wise
        _inv_bl_kernel.block(0, 0, _samples.size(), _samples.size()) = A1 + A1 * B * comA1 * B.transpose() * A1;
        _inv_bl_kernel.block(0, _samples.size(), _samples.size(), _bl_samples.size()) = -A1 * B * comA1;
        _inv_bl_kernel.block(_samples.size(), 0, _bl_samples.size(), _samples.size()) = _inv_bl_kernel.block(0, _samples.size(), _samples.size(), _bl_samples.size()).transpose();
        _inv_bl_kernel.block(_samples.size(), _samples.size(), _bl_samples.size(), _bl_samples.size()) = comA1;
      } 
187

188
      ObsType _mu(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
189
        return  (k.transpose() * _alpha) + _mean_function(v, *this).transpose();
190
      }
191

192
      double _sigma(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
193
194
195
196
197
198
199
200
201
        double res;
        if(_bl_samples.size() == 0) {
          Eigen::VectorXd z = _llt.matrixL().solve(k);
          res = _kernel_function(v, v) - z.dot(z);
        } else {
          res = _kernel_function(v, v) - k.transpose() * _inv_bl_kernel * k;
        }

        return (res <= std::numeric_limits<double>::epsilon()) ? 0 : res;
202
      }
203

204
205
206
207
208
209
      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;
      }
210

211
212
213
214
215
216
217
218
219
220
221
222
      Eigen::VectorXd _compute_k_bl(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {        
        if(_bl_samples.size() == 0) {
          return k;
        }

        Eigen::VectorXd k_bl(_samples.size() + _bl_samples.size());

        k_bl.head(_samples.size()) = k;
        for (size_t i = 0; i < _bl_samples.size(); i++)
          k_bl[i+this->_samples.size()] = this->_kernel_function(_bl_samples[i], v);
        return k_bl;
      }
223
224
225
    };
  }
}
226
227

#endif