Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

least_squares.cc

Go to the documentation of this file.
00001 #include "least_squares.h"
00002 #include <limits>
00003 
00004 /*
00005 Least Squares Solver
00006 by Ethan Tira-Thompson, © 2011
00007 
00008 least_squares is free software: you can redistribute it and/or modify
00009 it under the terms of the Lesser GNU General Public License as published
00010 by the Free Software Foundation, either version 2 of the License, or
00011 (at your option) any later version, waiving any requirement to
00012 distribute the license document itself.
00013 
00014 least_squares is distributed in the hope that it will be useful,
00015 but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 Lesser GNU General Public License for more details.
00018 */
00019 
00020 namespace least_squares {
00021 
00022 // can't typedef a template, so use macro :-P
00023 #define V(T) std::valarray< T >
00024 #define M(T) std::valarray< V(T) >
00025 
00026 template<class T>
00027 void invert(M(T)& m) {
00028   const size_t DIM=m.size();
00029 #ifdef DEBUG
00030   for(size_t i=0; i<DIM; ++i)
00031     if(m[i].size()!=DIM)
00032       throw std::invalid_argument("invert() of non-square matrix");
00033 #endif
00034   
00035   M(T) x(V(T)(DIM*2),DIM);
00036   for(size_t c=0; c<DIM; ++c) // copy m in top half
00037     x[c][std::slice(0,DIM,1)] = m[c];
00038   for(unsigned int c=0; c<DIM; ++c) // identity (to be inverse) in bottom half
00039     x[c][DIM+c] = 1;
00040   
00041   for(unsigned int c=0; c<DIM; ++c) {
00042     // find pivot
00043     T mx=std::abs(x[c][c]);
00044     size_t mxi=c;
00045     for(unsigned int r=c+1; r<DIM; ++r) {
00046       const T v=std::abs(x[r][c]);
00047       if(v>mx) {
00048         mx=v;
00049         mxi=r;
00050       }
00051     }
00052     if(mx<std::numeric_limits<float>::epsilon())
00053       throw std::underflow_error("low rank matrix, non-invertable");
00054     
00055     // swap to put pivot in position
00056     std::swap(x[c],x[mxi]);
00057     
00058     {
00059       const T p=x[c][c];
00060       for(size_t c2=c+1; c2<DIM*2; ++c2)
00061         x[c][c2] /= p;
00062       x[c][c] = 1;
00063     }
00064     
00065     for(size_t r=c+1; r<DIM; ++r) {
00066       const T p = x[r][c];
00067       for(size_t c2=c+1; c2<DIM*2; ++c2)
00068         x[r][c2] -= x[c][c2]*p;
00069       x[r][c] = 0;
00070     }
00071   }
00072   for(size_t c=0; c<DIM; ++c) {
00073     for(size_t r=0; r<c; ++r) {
00074       const T p = x[r][c];
00075       for(size_t c2=c+1; c2<DIM*2; ++c2)
00076         x[r][c2] -= x[c][c2]*p;
00077       x[r][c] = 0;
00078     }
00079   }
00080   
00081   for(size_t c=0; c<DIM; ++c)
00082     m[c] = x[c][std::slice(DIM,DIM,1)];
00083 }
00084 template void invert(M(float)& m);
00085 template void invert(M(double)& m);
00086 
00087 
00088 template<class T>
00089 V(T) linearFit(const M(T)& x, const V(T)& y) {
00090 #ifdef DEBUG
00091   if(x.size()==0) {
00092     throw std::invalid_argument("linearFit(): empty x");
00093   }
00094   for(size_t i=1; i<x.size(); ++i) {
00095     if(x[0].size()!=x[i].size()) {
00096       throw std::invalid_argument("linearFit(): x has uneven columns");
00097     }
00098   }
00099   if(x[0].size()!=y.size()) {
00100     throw std::invalid_argument("linearFit(): x and y length mismatch");
00101   }
00102 #endif
00103   const size_t N=x.size();
00104   
00105   // first compute x' * x:
00106   const V(T) vn(N);
00107   M(T) p(vn,N);
00108   for(size_t i=0; i<N; ++i) {
00109     for(size_t j=0; j<=i; ++j) {
00110       p[j][i] = p[i][j] = (x[i]*x[j]).sum(); //std::inner_product(x[i].begin(),x[i].end(),x[j].begin(),0);
00111     }
00112   }
00113   
00114   invert(p); // invert that
00115   
00116   // could multiply by x' and then y in turn...
00117   /*
00118   std::vector<V(T) > a(N,x[0]);
00119   for(size_t i=0; i<N; ++i) {
00120     a[i] *= p[i][0];
00121     for(size_t j=1; j<N; ++j) {
00122       a[i] += x[j]*p[i][j];
00123     }
00124   }
00125   V(T) ans(N);
00126   for(size_t i=0; i<N; ++i) {
00127     ans[i] = (a[i]*y).sum(); //std::inner_product(a[i].begin(),a[i].end(),y.begin(),0);
00128   }
00129   return ans;
00130   */
00131   
00132   // ... but faster to find x' * y first, then apply that to p
00133   V(T) r(N);
00134   for(size_t i=0; i<N; ++i)
00135     r[i] = (x[i]*y).sum();
00136   
00137   V(T) acc = p[0]*r[0];
00138   for(size_t i=1; i<N; ++i)
00139     acc += p[i]*r[i];
00140   return acc;
00141 }
00142 template V(float) linearFit(const M(float)& x, const V(float)& y);
00143 template V(double) linearFit(const M(double)& x, const V(double)& y);
00144 
00145 }
00146 
00147 #ifdef LEAST_SQUARES_TEST_MAIN
00148 #include <iostream>
00149 #include <vector>
00150 using namespace std;
00151 
00152 template<class T> ostream& operator<<(ostream& os, V(T) a) {
00153   for(size_t i=0; i<a.size(); ++i)
00154     os << a[i] << ' ';
00155   return os;
00156 }
00157 
00158 void runtest() {
00159   size_t S=100;
00160   
00161   {
00162     std::vector<float> x(S), y(S);
00163     float s=0;
00164     for(size_t i=0; i<S; ++i) {
00165       float t = float(i)/S*2-1;
00166       x[i] = t;
00167       y[i] = random() / float(1<<31);
00168       s+=y[i];
00169     }
00170     {
00171       cout << "noise average: ";
00172       V(float) a = least_squares::polyFit(0,x,y);
00173       cout << a << "vs " << s/S << endl;
00174     }
00175     {
00176       cout << "noise fit: ";
00177       V(float) a = least_squares::polyFit(1,x,y);
00178       cout << a << endl;
00179     }
00180   }
00181   
00182   valarray<valarray<float> > x(valarray<float>(S),3);
00183   valarray<float> y(S);
00184   
00185   float m=3.214;
00186   float b=1.2;
00187   
00188   for(size_t i=0; i<S; ++i) {
00189     float t = float(i)/S*2-1;
00190     x[0][i] = 1;
00191     x[1][i] = t;
00192     x[2][i] = t*t;
00193     //y[i] = m*t + b;
00194   }
00195   y = m*x[1] + b*x[0];
00196   
00197   {
00198     cout << "linear fit: ";
00199     V(float) a = least_squares::linearFit(x,y);
00200     cout << a << endl;
00201   }
00202   
00203   {
00204     cout << "poly fit:   ";
00205     V(float) a = least_squares::polyFit(2, x[1], y);
00206     cout << a << endl;
00207     
00208     cout << "poly abs error sum: ";
00209     cout << abs(least_squares::polyApply(a,x[1]) - y).sum() << endl;
00210   }
00211   
00212   float arrf[] = { 1, 2, 3 };
00213   std::valarray<float> vaf(arrf,3);
00214   std::vector<float> vef(arrf,arrf+3);
00215   V(float) t1 = least_squares::polyApply(vaf,vef);
00216   cout << t1 << endl;
00217   V(float) t2 = least_squares::polyApply(vef,vaf);
00218   cout << t2 << endl;
00219   
00220   /*cout << "[ ";
00221   for(unsigned int i=0; i<4; ++i) {
00222     for(unsigned int j=0; j<4; ++j) {
00223       m[j][i] = random() / float(1<<31);
00224       f(i,j) = m[j][i];
00225       cout << m[j][i] << (j==3?";":", "); 
00226     }
00227     cout << (i==3?"]\n":"\n");
00228   }*/
00229   
00230 }
00231 
00232 int main(int argc, char** argv) {
00233   try {
00234     runtest();
00235   } catch(const std::exception& ex) {
00236     cerr << ex.what() << endl;
00237     return 1;
00238   }
00239   return 0;
00240 }
00241 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:43 2016 by Doxygen 1.6.3