gp.hpp 11 KB
Newer Older
1
2
#ifndef LIMBO_MODEL_GP_HPP
#define LIMBO_MODEL_GP_HPP
3

4
#include <iostream>
5
#include <cassert>
6
#include <limits>
7
#include <vector>
8

9
10
#include <boost/parameter.hpp>

11
12
13
14
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Cholesky>

15
16
#include <limbo/opt/impl/model_no_opt.hpp>

17
namespace limbo {
18
19
20

    BOOST_PARAMETER_TEMPLATE_KEYWORD(optfun)

21
    namespace model {
22
23
24
25

        typedef boost::parameter::parameters<boost::parameter::optional<tag::optfun>> gp_signature;

        template <typename Params, typename KernelFunction, typename MeanFunction, class OptFun = boost::parameter::void_>
26
        class GP {
27
        public:
28
29
30
31
32
33
34
35
            // defaults
            struct defaults {
                typedef opt::impl::ModelNoOpt<Params> opt_t; // 1
            };

            typedef typename gp_signature::bind<OptFun>::type args;
            typedef typename boost::parameter::binding<args, tag::optfun, typename defaults::opt_t>::type opt_t;

36
37
38
39
40
41
            GP() : _dim_in(-1), _dim_out(-1) {}
            // useful because the model might be created  before having samples
            GP(int dim_in, int dim_out)
                : _dim_in(dim_in), _dim_out(dim_out), _kernel_function(dim_in), _mean_function(dim_out) {}

            void compute(const std::vector<Eigen::VectorXd>& samples,
42
                const std::vector<Eigen::VectorXd>& observations, double noise,
43
44
                const std::vector<Eigen::VectorXd>& bl_samples = std::vector<Eigen::VectorXd>())
            {
45
                if (_dim_in == -1) {
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
                    assert(samples.size() != 0);
                    assert(observations.size() != 0);
                    assert(samples.size() == observations.size());
                    _dim_in = samples[0].size();
                    _dim_out = observations[0].size();
                }

                _samples = samples;

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

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

                _noise = noise;

                _bl_samples = bl_samples;

                _compute_obs_mean();
                _compute_kernel();
69
70

                opt_t()(*this);
71
72
73
            }

            // return mu, sigma (unormaliz)
74
            std::tuple<Eigen::VectorXd, double> query(const Eigen::VectorXd& v) const
75
76
77
78
79
80
81
82
83
84
85
86
87
            {
                if (_samples.size() == 0 && _bl_samples.size() == 0)
                    return std::make_tuple(_mean_function(v, *this),
                        sqrt(_kernel_function(v, v)));

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

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

88
            Eigen::VectorXd mu(const Eigen::VectorXd& v) const
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
            {
                if (_samples.size() == 0)
                    return _mean_function(v, *this);
                return _mu(v, _compute_k(v));
            }

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

            int dim_in() const
            {
                assert(_dim_in != -1); // need to compute first !
                return _dim_in;
            }

            int dim_out() const
            {
                assert(_dim_out != -1); // need to compute first !
                return _dim_out;
            }

            const KernelFunction& kernel_function() const { return _kernel_function; }

116
117
            KernelFunction& kernel_function() { return _kernel_function; }

118
119
            const MeanFunction& mean_function() const { return _mean_function; }

120
121
            MeanFunction& mean_function() { return _mean_function; }

122
            Eigen::VectorXd max_observation() const
123
124
125
126
127
128
129
            {
                if (_observations.cols() > 1)
                    std::cout << "WARNING max_observation with multi dim_inensional "
                                 "observations doesn't make sense" << std::endl;
                return _observations.maxCoeff();
            }

130
            Eigen::VectorXd mean_observation() const
131
            {
132
                // TO-DO: Check if _dim_out is correct?!
133
                return _samples.size() > 0 ? _mean_observation
134
                                           : Eigen::VectorXd::Zero(_dim_out);
135
136
137
138
            }

            const Eigen::MatrixXd& mean_vector() const { return _mean_vector; }

139
            const Eigen::MatrixXd& obs_mean() const { return _obs_mean; }
140

141
142
143
144
            int nb_samples() const { return _samples.size(); }

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

145
146
147
148
149
150
151
152
153
154
            void update()
            {
                this->_compute_obs_mean(); // ORDER MATTERS
                this->_compute_kernel();
            }

            float get_lik() const { return _lik; }

            void set_lik(const float& lik) { _lik = lik; }

155
            const Eigen::LLT<Eigen::MatrixXd>& llt() const { return _llt; }
156

157
            const Eigen::MatrixXd& alpha() const { return _alpha; }
158

159
            const std::vector<Eigen::VectorXd>& samples() const { return _samples; }
160

161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
        protected:
            int _dim_in;
            int _dim_out;

            KernelFunction _kernel_function;
            MeanFunction _mean_function;

            std::vector<Eigen::VectorXd> _samples;
            Eigen::MatrixXd _observations;
            std::vector<Eigen::VectorXd> _bl_samples; // black listed samples
            Eigen::MatrixXd _mean_vector;
            Eigen::MatrixXd _obs_mean;

            double _noise;
            Eigen::MatrixXd _alpha;
176
            Eigen::VectorXd _mean_observation;
177
178
179
180
181
182
183

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

184
185
            float _lik;

186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
            void _compute_obs_mean()
            {
                _mean_vector.resize(_samples.size(), _dim_out);
                for (int i = 0; i < _mean_vector.rows(); i++)
                    _mean_vector.row(i) = _mean_function(_samples[i], *this);
                _obs_mean = _observations - _mean_vector;
            }

            void _compute_kernel()
            {
                // O(n^2) [should be negligible]
                _kernel.resize(_samples.size(), _samples.size());
                for (int i = 0; i < _samples.size(); i++)
                    for (int j = 0; j < _samples.size(); ++j)
                        _kernel(i, j) = _kernel_function(_samples[i], _samples[j]) + ((i == j) ? _noise : 0); // noise only on the diagonal

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

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

                // alpha = K^{-1} * this->_obs_mean;
                _alpha = _llt.matrixL().solve(_obs_mean);
                _llt.matrixL().adjoint().solveInPlace(_alpha);
                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;
            }

246
            Eigen::VectorXd _mu(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const
247
248
249
250
251
252
253
            {
                return (k.transpose() * _alpha) + _mean_function(v, *this).transpose();
            }

            double _sigma(const Eigen::VectorXd& v, const Eigen::VectorXd& k) const
            {
                double res;
254
                if (_bl_samples.size() == 0) {
255
256
257
                    Eigen::VectorXd z = _llt.matrixL().solve(k);
                    res = _kernel_function(v, v) - z.dot(z);
                }
258
                else {
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
                    res = _kernel_function(v, v) - k.transpose() * _inv_bl_kernel * k;
                }

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

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

            Eigen::VectorXd _compute_k_bl(const Eigen::VectorXd& v,
                const Eigen::VectorXd& k) const
            {
276
                if (_bl_samples.size() == 0) {
277
278
279
280
281
282
283
284
285
286
287
288
                    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;
            }
        };
    }
289
}
290

291
#endif