Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <Eigen/Core>
00040 #include <Eigen/Cholesky>
00041
00042 #include <stdio.h>
00043 #include <sys/time.h>
00044 #include <unistd.h>
00045
00046 #include <iostream>
00047 #include <fstream>
00048 #include <vector>
00049
00050 #include "inverse_dynamics/utils.h"
00051
00052 #include <optimization.h>
00053
00054 #define _USE_MATH_DEFINES
00055 #include <cmath>
00056
00057 using namespace Eigen;
00058
00059 class GPRModel {
00060
00061 private:
00062 MatrixXd X;
00063 MatrixXd Y;
00064
00065 VectorXd alpha;
00066
00067 std::vector<double> hyp;
00068
00069 double nlZ;
00070 std::vector<double> dnlZ;
00071
00072 MatrixXd k(MatrixXd A, MatrixXd B);
00073
00074 public:
00075 GPRModel();
00076 void setParameters();
00077 void inference(MatrixXd X, VectorXd Y);
00078 double predict(VectorXd x);
00079 };
00080
00081 GPRModel::GPRModel() {
00082 hyp.resize(3);
00083 hyp[0] = 0.0;
00084 hyp[1] = 0.0;
00085 hyp[2] = log(0.1);
00086 nlZ = 0.0;
00087 dnlZ.resize(3);
00088 }
00089
00090 void GPRModel::setParameters(std::vector<double> params) {
00091 hyp = params;
00092 }
00093
00094 std::vector<double> GPRModel::getHyp() {
00095 return hyp;
00096 }
00097
00098 MatrixXd GPRModel::k(MatrixXd A, MatrixXd B) {
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108 double ell = exp(hyp[0]);
00109 double sf2 = exp(hyp[1] * 2.0);
00110
00111 A /= ell;
00112 B /= ell;
00113
00114 MatrixXd K = MatrixXd::Zero(A.rows(), B.rows());
00115
00116
00117
00118 MatrixXd x = A.array().pow(2).rowwise().sum();
00119 for (int i = 0; i < K.cols(); i++) {
00120 K.col(i) += x;
00121 }
00122
00123 x = B.array().pow(2).rowwise().sum().transpose();
00124 for (int i = 0; i < K.rows(); i++) {
00125 K.row(i) += x;
00126 }
00127
00128 K -= 2.0 * A * B.transpose();
00129
00130
00131 K = K.array().abs();
00132
00133 K /= -2.0;
00134 K = K.array().exp() * sf2;
00135
00136 return K;
00137 }
00138
00139 MatrixXd sq_dist(MatrixXd A, MatrixXd B) {
00140 MatrixXd K = MatrixXd::Zero(A.rows(), B.rows());
00141
00142
00143
00144 MatrixXd x = A.array().pow(2).rowwise().sum();
00145 for (int i = 0; i < K.cols(); i++) {
00146 K.col(i) += x;
00147 }
00148
00149 x = B.array().pow(2).rowwise().sum().transpose();
00150 for (int i = 0; i < K.rows(); i++) {
00151 K.row(i) += x;
00152 }
00153
00154 K -= 2.0 * A * B.transpose();
00155
00156
00157 K = K.array().abs();
00158
00159 return K;
00160 }
00161
00162 VectorXd GPRModel::inference(MatrixXd X, VectorXd Y) {
00163 double ell = exp(hyp[0]);
00164 double sf2 = exp(hyp[1] * 2.0);
00165 double sn2 = exp(hyp[3] * 2.0);
00166
00167
00168
00169 int n = X.rows();
00170 int D = X.cols();
00171
00172 VectorXd alpha;
00173
00174
00175 MatrixXd K;
00176 K = k(X);
00177
00178
00179 VectorXd m = VectorXd::Zero(n);
00180
00181
00182 Eigen::LLT<MatrixXd> LLT;
00183 LLT = (K / sn2 + MatrixXd::Identity(n, n)).llt();
00184 alpha = LLT.solve(Y - m);
00185 alpha /= sn2;
00186
00187
00188 MatrixXd L = LLT.matrixL();
00189
00190 hyp.nlZ = (((Y - m).transpose() * alpha / 2.0).array()
00191 + L.diagonal().array().log().sum())[0] + n * log(2 * M_PI * sn2) / 2.0;
00192
00193 printf("[infExact] nlZ: %.5f\n", hyp.nlZ);
00194
00195
00196
00197 K = sq_dist(X / ell, X / ell);
00198
00199
00200 MatrixXd Q = LLT.solve(MatrixXd::Identity(n, n)) / sn2 - alpha * alpha.transpose();
00201
00202
00203 MatrixXd K1 = sf2 * (K / -2.0).array().exp() * K.array();
00204 hyp.dnlZ[0] = (Q.array() * K1.array()).sum() / 2.0;
00205
00206
00207
00208 MatrixXd K2 = 2.0 * sf2 * (K / -2.0).array().exp();
00209 hyp.dnlZ[1] = (Q.array() * K2.array()).sum() / 2.0;
00210
00211
00212
00213 hyp.dnlZ[2] = sn2 * Q.diagonal().sum();
00214
00215
00216 return alpha;
00217 }
00218
00219 void cb_minimize(const alglib::real_1d_array &x, double &func, alglib::real_1d_array &grad, void *ptr) {
00220 hyp.cov[0] = x[0];
00221 hyp.cov[1] = x[1];
00222 hyp.lik = x[2];
00223 VectorXd alpha = infExact(myX, myY);
00224 func = hyp.nlZ;
00225 grad[0] = hyp.dnlZ[0];
00226 grad[1] = hyp.dnlZ[1];
00227 grad[2] = hyp.dnlZ[2];
00228 }
00229
00230
00231 double GPRModel::predict(VectorXd x) {
00232 MatrixXd ks;
00233 MatrixXd xs;
00234 xs = x.transpose();
00235 ks = k(X, xs);
00236 VectorXd v = (ks.transpose() * alpha);
00237 return v(0);
00238 }
00239
00240 int main(int, char *[])
00241 {
00242 MatrixXd data;
00243 utils::readMatrix("../data/test.txt", &data);
00244
00245
00246
00247
00248 alglib::real_1d_array x="[0,0,0]";
00249 x[0] = hyp.cov[0];
00250 x[1] = hyp.cov[1];
00251 x[2] = hyp.lik;
00252 double epsg = 0.00001;
00253 double epsf = 0;
00254 double epsx = 0;
00255 alglib::ae_int_t maxits = 100;
00256 alglib::minlbfgsstate state;
00257 alglib::minlbfgsreport rep;
00258
00259 alglib::minlbfgscreate(1, x, state);
00260 alglib::minlbfgssetcond(state, epsg, epsf, epsx, maxits);
00261 alglib::minlbfgsoptimize(state, cb_minimize);
00262 alglib::minlbfgsresults(state, x, rep);
00263
00264 int const tt = int(rep.terminationtype);
00265 switch (tt) {
00266 case 4:
00267 printf("[minlbfgs] gradient norm <= %.5f.\n", epsg);
00268 break;
00269 case 5:
00270 printf("[minlbfgs] max # iterations (%d) reached.\n", int(maxits));
00271 break;
00272 }
00273 printf("\nhyperparams: %.5f, %.5f, %.5f\n", hyp.cov[0], hyp.cov[1], hyp.lik);
00274
00275 }