radial_approx.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2012, Scott Niekum
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Robert Bosch nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  *********************************************************************/
00036 
00042 #include "dmp/radial_approx.h"
00043 #include<stdio.h>
00044 using namespace Eigen;
00045 using namespace std;
00046 
00047 namespace dmp{
00048 
00049 RadialApprox::RadialApprox(int num_bases, double base_width, double alpha)
00050 {
00051         n_bases = num_bases;
00052         features = new double[n_bases];
00053         centers = new double[n_bases];
00054         widths = new double[n_bases];
00055         weights.resize(n_bases);
00056         for(int i=0; i<n_bases; i++){
00057                 features[i] = 0;
00058                 centers[i] = exp((-alpha*i)/n_bases);
00059                 widths[i] = base_width * (1/exp((-alpha*i)/n_bases));
00060         }
00061 }
00062 
00063 
00064 RadialApprox::RadialApprox(const vector<double> &w, double base_width, double alpha)
00065 {
00066         weights = w;
00067         n_bases = w.size();
00068         features = new double[n_bases];
00069                 centers = new double[n_bases];
00070                 widths = new double[n_bases];
00071                 for(int i=0; i<n_bases; i++){
00072                         features[i] = 0;
00073                         centers[i] = ((double)i)/((double)n_bases);  //exp((-alpha*i)/n_bases);
00074                         widths[i] = base_width; //base_width * exp((-alpha*i)/n_bases);
00075                 }
00076 }
00077 
00078 
00079 RadialApprox::~RadialApprox()
00080 {
00081         delete[] features;
00082         delete[] centers;
00083         delete[] widths;
00084 }
00085 
00086 
00087 double RadialApprox::evalAt(double x)
00088 {
00089         calcFeatures(x);
00090 
00091         double wsum = 0;
00092         for(int i=0; i<n_bases; i++){
00093                 wsum += features[i] * weights[i];
00094         }
00095         return wsum;
00096 }
00097 
00098 
00099 void RadialApprox::leastSquaresWeights(double *X, double *Y, int n_pts)
00100 {
00101         MatrixXd D_mat = MatrixXd(n_pts,n_bases);
00102         MatrixXd Y_mat = MatrixXd(n_pts,1);
00103 
00104         //Calculate the design matrix
00105         for(int i=0; i<n_pts; i++){
00106                 Y_mat(i,0) = Y[i];
00107                 calcFeatures(X[i]);
00108                 for(int j=0; j<n_bases; j++){
00109                         D_mat(i,j) = features[j];
00110                 }
00111         }
00112 
00113         //Calculate the least squares weights via projection onto the basis functions
00114         MatrixXd w = pseudoinverse(D_mat.transpose() * D_mat) * D_mat.transpose() * Y_mat;
00115         for(int i=0; i<n_bases; i++){
00116                 weights[i] = w(i,0);
00117         }
00118 }
00119 
00120 
00121 void RadialApprox::calcFeatures(double x)
00122 {
00123         double sum = 0;
00124         for(int i=0; i<n_bases; i++){
00125                 features[i] = exp(-widths[i]*(x-centers[i])*(x-centers[i]));
00126                 sum += features[i];
00127         }
00128         for(int i=0; i<n_bases; i++){
00129                 features[i] /= sum;
00130         }
00131 }
00132 
00133 
00134 MatrixXd RadialApprox::pseudoinverse(MatrixXd mat){
00135         //Numpy uses 1e-15 by default.  I use 1e-10 just to be safe.
00136         double precisionCutoff = 1e-10;
00137 
00138         //Compute the SVD of the matrix
00139         JacobiSVD<MatrixXd> svd(mat, ComputeThinU | ComputeThinV);
00140         MatrixXd U = svd.matrixU();
00141         MatrixXd V = svd.matrixV();
00142         MatrixXd S = svd.singularValues();
00143 
00144         //Psuedoinvert the diagonal matrix of singular values
00145         MatrixXd S_plus = MatrixXd::Zero(n_bases, n_bases);
00146         for(int i=0; i<n_bases; i++){
00147                 if(S(i) > precisionCutoff){  //Cutoff to avoid huge inverted values for numerical stability
00148                         S_plus(i,i) = 1.0/S(i);
00149                 }
00150         }
00151 
00152         //Compute psuedoinverse of orginal matrix
00153         return V * S_plus * U.transpose();
00154 }
00155 
00156 }
00157 
00158 


dmp
Author(s): Scott Niekum
autogenerated on Sun Oct 5 2014 23:29:12