fourier_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/fourier_approx.h"
00043 #include<stdio.h>
00044 using namespace Eigen;
00045 using namespace std;
00046 
00047 namespace dmp{
00048 
00049 FourierApprox::FourierApprox(int order)
00050 {
00051         n_bases = order + 1;     //Univariate Fourier has order-many bases plus a constant basis
00052         features = new double[n_bases];
00053         weights.resize(n_bases);
00054         for(int i=0; i<n_bases; i++){
00055                 features[i] = 0;
00056         }
00057 }
00058 
00059 
00060 FourierApprox::FourierApprox(const vector<double> &w)
00061 {
00062         weights = w;
00063         n_bases = w.size();
00064         features = new double[n_bases];
00065         for(int i=0; i<n_bases; i++){
00066                 features[i] = 0;
00067         }
00068 }
00069 
00070 
00071 FourierApprox::~FourierApprox()
00072 {
00073         delete[] features;
00074 }
00075 
00076 
00077 double FourierApprox::evalAt(double x)
00078 {
00079         calcFeatures(x);
00080 
00081         double wsum = 0;
00082         for(int i=0; i<n_bases; i++){
00083                 wsum += features[i] * weights[i];
00084         }
00085         return wsum;
00086 }
00087 
00088 
00089 void FourierApprox::leastSquaresWeights(double *X, double *Y, int n_pts)
00090 {
00091         MatrixXd D_mat = MatrixXd(n_pts,n_bases);
00092         MatrixXd Y_mat = MatrixXd(n_pts,1);
00093 
00094         //Calculate the design matrix
00095         for(int i=0; i<n_pts; i++){
00096                 Y_mat(i,0) = Y[i];
00097                 calcFeatures(X[i]);
00098                 for(int j=0; j<n_bases; j++){
00099                         D_mat(i,j) = features[j];
00100                 }
00101         }
00102 
00103         //Calculate the least squares weights via projection onto the basis functions
00104         MatrixXd w = pseudoinverse(D_mat.transpose() * D_mat) * D_mat.transpose() * Y_mat;
00105         for(int i=0; i<n_bases; i++){
00106                 weights[i] = w(i,0);
00107         }
00108 }
00109 
00110 
00111 void FourierApprox::calcFeatures(double x)
00112 {
00113         for(int i=0; i<n_bases; i++){
00114                 features[i] = cos(PI*i*x);
00115         }
00116 }
00117 
00118 
00119 MatrixXd FourierApprox::pseudoinverse(MatrixXd mat){
00120         //Numpy uses 1e-15 by default.  I use 1e-10 just to be safe.
00121         double precisionCutoff = 1e-10;
00122 
00123         //Compute the SVD of the matrix
00124         JacobiSVD<MatrixXd> svd(mat, ComputeThinU | ComputeThinV);
00125         MatrixXd U = svd.matrixU();
00126         MatrixXd V = svd.matrixV();
00127         MatrixXd S = svd.singularValues();
00128 
00129         //Psuedoinvert the diagonal matrix of singular values
00130         MatrixXd S_plus = MatrixXd::Zero(n_bases, n_bases);
00131         for(int i=0; i<n_bases; i++){
00132                 if(S(i) > precisionCutoff){  //Cutoff to avoid huge inverted values for numerical stability
00133                         S_plus(i,i) = 1.0/S(i);
00134                 }
00135         }
00136 
00137         //Compute psuedoinverse of orginal matrix
00138         return V * S_plus * U.transpose();
00139 }
00140 
00141 }
00142 
00143 


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