gp.hpp 4.79 KB
Newer Older
1

2
3
4
5
6
7
8
9
10
11
12
13
14
#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 {
15
16
17


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

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

28
29
30
31
32
33
34
        if (_dim == -1) {
          assert(samples.size() != 0);
          assert(observations.size() != 0);
          assert(samples.size() == observations.size());
          _dim = samples[0].size();
        }
        _samples = samples;
35
        _observations.resize(observations.size(),observations[0].size());
36
37
        _noise = noise;

38
	int obs_dim=observations[0].size();
39

40
41
42
43
44
45
46
47
48
        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++)
	  _mean_vector.row(i) = ObsType::Zero(obs_dim)+_mean_function(_samples[i], *this); // small trick to accept either Double or Vector
49
50
        _obs_mean = _observations - _mean_vector;

51

52
        _compute_kernel();
53

54
55
      }

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

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

66
      ObsType mu(const Eigen::VectorXd& v) const {
67
        if (_samples.size() == 0)
68
          return _mean_function(v, *this);
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
        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;
      }
86
87
88
      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
89
90
        return _observations.maxCoeff();
      }
91
      ObsType mean_observation() const {
92
        return _mean_observation;
93
      }
94
95
96
97
98
99
     protected:
      int _dim;
      KernelFunction _kernel_function;
      MeanFunction _mean_function;

      std::vector<Eigen::VectorXd> _samples;
100
101
102
      Eigen::MatrixXd _observations;
      Eigen::MatrixXd _mean_vector;
      Eigen::MatrixXd _obs_mean;
103

104
      double _noise;
105
106
      Eigen::MatrixXd _alpha;
      ObsType _mean_observation;
107
108
109
110
111
112
113
114

      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]
115
116
117
        _kernel.resize(_samples.size(), _samples.size());
        for (size_t i = 0; i < _samples.size(); i++)
          for (size_t j = 0; j < _samples.size(); ++j)
118
119
120
            _kernel(i, j) = _kernel_function(_samples[i], _samples[j]) + _noise;

        // O(n^3)
121
        //  _inverted_kernel = _kernel.inverse();
122
123
124
125
126
127

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

129
130
      }

131
132
      ObsType _mu(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const {
        return  (k.transpose() * _alpha) + _mean_function(v, *this);
Jean-Baptiste Mouret's avatar
Jean-Baptiste Mouret committed
133
134
        //        return _mean_function(v)
        //               + (k.transpose() * _inverted_kernel * (_obs_mean))[0];
135
136
137
138
      }
      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
139
        //        return  _kernel_function(v, v) - (k.transpose() * _inverted_kernel * k)[0];
140
141
142
143
144
145
146
147
148
149
150
      }
      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