GestureRecognitionToolkit  Version: 0.1.0
The Gesture Recognition Toolkit (GRT) is a cross-platform, open-source, c++ machine learning library for real-time gesture recognition.
LUDecomposition.cpp
1 /*
2  GRT MIT License
3  Copyright (c) <2012> <Nicholas Gillian, Media Lab, MIT>
4 
5  Permission is hereby granted, free of charge, to any person obtaining a copy of this software
6  and associated documentation files (the "Software"), to deal in the Software without restriction,
7  including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
8  and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
9  subject to the following conditions:
10 
11  The above copyright notice and this permission notice shall be included in all copies or substantial
12  portions of the Software.
13 
14  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
15  LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
16  IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
17  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
18  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
19 
20  This code is based on the LU Decomposition code from Numerical Recipes (3rd Edition)
21 
22  */
23 
24 #include "LUDecomposition.h"
25 
26 GRT_BEGIN_NAMESPACE
27 
28 LUDecomposition::LUDecomposition(const MatrixFloat &a) : sing(false){
29 
30  debugLog.setProceedingText("[DEBUG LUDecomposition]");
31  errorLog.setProceedingText("[ERROR LUDecomposition]");
32  warningLog.setProceedingText("[WARNING LUDecomposition]");
33 
34  N = a.getNumRows();
35  lu = a;
36  aref = a;
37  indx.resize( N );
38 
39  const Float TINY=1.0e-20;
40  unsigned int i,imax,j,k;
41  Float big,temp;
42  VectorFloat vv(N);
43  d=1.0;
44  imax = 0;
45  for (i=0;i<N;i++) {
46  big=0.0;
47  for (j=0;j<N;j++)
48  if ((temp=fabs( lu[i][j] )) > big) big=temp;
49  if (big == 0.0){
50  sing = true;
51  errorLog << "Error in LUDecomposition constructor, big == 0.0" << std::endl;
52  return;
53  }
54  vv[i] = 1.0/big;
55  }
56  for (k=0;k<N;k++) {
57  big=0.0;
58  for (i=k;i<N;i++) {
59  temp=vv[i]*fabs(lu[i][k]);
60  if (temp > big) {
61  big=temp;
62  imax=i;
63  }
64  }
65  if (k != imax) {
66  for (j=0;j<N;j++) {
67  temp=lu[imax][j];
68  lu[imax][j] = lu[k][j];
69  lu[k][j] = temp;
70  }
71  d = -d;
72  vv[imax]=vv[k];
73  }
74  indx[k]=imax;
75  if (lu[k][k] == 0.0) lu[k][k] = TINY;
76  for (i=k+1; i<N; i++) {
77  temp = lu[i][k] /= lu[k][k];
78  for (j=k+1;j<N;j++)
79  lu[i][j] -= temp * lu[k][j];
80  }
81  }
82 
83 }
84 
85 LUDecomposition::~LUDecomposition(){
86 
87 }
88 
89 bool LUDecomposition::solve_vector(const VectorFloat &b,VectorFloat &x)
90 {
91  int i=0,ii=0,ip=0,j=0; //This must be an int (as opposed to an UINT)
92  const int n = int(N);
93  Float sum=0;
94 
95  if (b.size() != N || x.size() != N){
96  errorLog << "solve_vector(const VectorFloat &b,VectorFloat &x) - the size of the two vectors does not match!" << std::endl;
97  return false;
98  }
99  for (i=0;i<n;i++) x[i] = b[i];
100  for (i=0;i<n;i++) {
101  ip=indx[i];
102  sum=x[ip];
103  x[ip] = x[i];
104  if (ii != 0)
105  for (j=ii-1;j<i;j++) sum -= lu[i][j] * x[j];
106  else if (sum != 0.0)
107  ii=i+1;
108  x[i]=sum;
109  }
110  for (i=n-1;i>=0;i--) {
111  sum=x[i];
112  for (j=i+1;j<n;j++) sum -= lu[i][j] * x[j];
113  x[i] = sum / lu[i][i];
114  }
115 
116  return true;
117 }
118 
119 bool LUDecomposition::solve(const MatrixFloat &b,MatrixFloat &x)
120 {
121  unsigned int m=b.getNumCols();
122  if (b.getNumRows() != N || x.getNumRows() != N || b.getNumCols() != x.getNumCols() ){
123  errorLog << "solve(const MatrixFloat &b,MatrixFloat &x) - the size of the two matrices does not match!" << std::endl;
124  return false;
125  }
126  VectorFloat xx(N);
127  for (unsigned int j=0; j<m; j++) {
128  for(unsigned int i=0; i<N; i++) xx[i] = b[i][j];
129  solve_vector(xx,xx);
130  for(unsigned int i=0; i<N; i++) x[i][j] = xx[i];
131  }
132  return true;
133 }
134 
135 bool LUDecomposition::inverse(MatrixFloat &ainv)
136 {
137  unsigned int i,j;
138  ainv.resize(N,N);
139  for (i=0;i<N;i++) {
140  for (j=0;j<N;j++) ainv[i][j] = 0.0;
141  ainv[i][i] = 1.0;
142  }
143  return solve(ainv,ainv);
144 }
145 
146 Float LUDecomposition::det()
147 {
148  Float dd = d;
149  for (unsigned int i=0;i<N;i++) dd *= lu[i][i];
150  return dd;
151 }
152 
153 bool LUDecomposition::mprove(const VectorFloat &b,VectorFloat &x)
154 {
155  unsigned int i,j;
156  VectorFloat r(N);
157  LongFloat sdp;
158  for (i=0;i<N;i++) {
159  sdp = -b[i];
160  for (j=0;j<N;j++)
161  sdp += (LongFloat) aref[i][j] * (LongFloat)x[j];
162  r[i]=sdp;
163  }
164  if( !solve_vector(r,r) ){
165  return false;
166  }
167  for (i=0;i<N;i++) x[i] -= r[i];
168  return true;
169 }
170 
171 bool LUDecomposition::getIsSingular(){
172  return sing;
173 }
174 
175 MatrixFloat LUDecomposition::getLU(){
176  return lu;
177 }
178 
179 GRT_END_NAMESPACE
unsigned int getNumRows() const
Definition: Matrix.h:542
unsigned int getNumCols() const
Definition: Matrix.h:549
virtual bool resize(const unsigned int r, const unsigned int c)
Definition: Matrix.h:232