gpr.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /*
00036   Author: Daniel Hennes
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; // support set
00063   MatrixXd Y;
00064 
00065   VectorXd alpha; // prediction vector
00066 
00067   std::vector<double> hyp; // hyperparameters
00068 
00069   double nlZ; // negative log-likelihood
00070   std::vector<double> dnlZ; // derivative of neg. log-likelihood
00071 
00072   MatrixXd k(MatrixXd A, MatrixXd B); // kernel function
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; // log(ell), lengthscale
00084   hyp[1] = 0.0; // log(sigma_f^2)
00085   hyp[2] = log(0.1); // log(sigma_n^2)
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      implements the following kernel function
00101      K = k(X,X)
00102      k = sf^2 * exp(-(x^p - x^q)'*inv(P)*(x^p - x^q)/2) 
00103      P = eye * el1^2
00104   */
00105 
00106   // lengthscale
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   // compute squared distance: (a-b)^2
00117   // a^2
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   // b^2
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   // -2 * a * b
00128   K -= 2.0 * A * B.transpose();
00129 
00130   // eliminate numerical erros (negative entries in matrix)
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   // compute squared distance: (a-b)^2
00143   // a^2
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   // b^2
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   // -2 * a * b
00154   K -= 2.0 * A * B.transpose();
00155 
00156   // eliminate numerical erros (negative entries in matrix)
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   // printf("[infExact] called with: %5.3f, %5.3f, %5.3f\n", ell, sf2, sn2);
00168 
00169   int n = X.rows();
00170   int D = X.cols();
00171 
00172   VectorXd alpha;
00173  
00174   // evaluate covariance matrix
00175   MatrixXd K;
00176   K = k(X);
00177   
00178   // evaluate mean vector
00179   VectorXd m = VectorXd::Zero(n);
00180 
00181   // Cholesky factor of covariance with noise
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   // negative marginal log-likelihood
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   // printf("negative log-likelihood: %6.4f\n", nlZ);
00193   printf("[infExact] nlZ: %.5f\n", hyp.nlZ);
00194 
00195   // squared distance
00196   // copy from function k
00197   K = sq_dist(X / ell, X / ell);
00198 
00199   // derivatives
00200   MatrixXd Q = LLT.solve(MatrixXd::Identity(n, n)) / sn2 - alpha * alpha.transpose();
00201 
00202   // hyp.ell
00203   MatrixXd K1 = sf2 * (K / -2.0).array().exp() * K.array();
00204   hyp.dnlZ[0] = (Q.array() * K1.array()).sum() / 2.0;
00205   // printf("dnlZ(ell): %6.4f\n", hyp.dnlZ[0]);
00206 
00207   // hyp.sf2
00208   MatrixXd K2 = 2.0 * sf2 * (K / -2.0).array().exp();
00209   hyp.dnlZ[1] = (Q.array() * K2.array()).sum() / 2.0;
00210   // printf("dnlZ(sf2): %6.4f\n", hyp.dnlZ[1]);
00211 
00212   // hyp.sn2
00213   hyp.dnlZ[2] = sn2 * Q.diagonal().sum();
00214   // printf("dnlZ(sn2): %6.4f\n", hyp.dnlZ[2]);
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   // data.col(0);
00246   // data.col(1);
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 }


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:03