ProteoWizard
MatrixInverse.hpp
Go to the documentation of this file.
1 //
2 // $Id: MatrixInverse.hpp 4146 2012-11-26 23:46:34Z pcbrefugee $
3 //
4 //
5 // NB: Variations of this file appear in many open source projects,
6 // with no copyright claims or license statements made in any of them.
7 // Assumed to be public domain, or at least as open as the boost license,
8 // since the farthest back we can seem to trace it is the Boost Wiki at
9 // http://www.crystalclearsoftware.com/cgi-bin/boost_wiki/wiki.pl?Effective_UBLAS/Matrix_Inversion
10 //
11 
12 #ifndef _MATRIXINVERSE_HPP_
13 #define _MATRIXINVERSE_HPP_
14 
15 #include <boost/numeric/ublas/matrix.hpp>
16 #include <boost/numeric/ublas/matrix_proxy.hpp>
17 #include <boost/numeric/ublas/io.hpp>
18 #include <boost/numeric/ublas/matrix_expression.hpp>
19 
20 #include <iostream>
21 #include <fstream>
22 #include <vector>
23 
24 #include <boost/numeric/ublas/vector.hpp>
25 #include <boost/numeric/ublas/vector_proxy.hpp>
26 #include <boost/numeric/ublas/triangular.hpp>
27 #include <boost/numeric/ublas/lu.hpp>
28 
29 /* Matrix inversion routine.
30 Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
31 template<typename M>
32 bool InvertMatrix(const M& input,
33  M& inverse)
34 {
35  using namespace boost::numeric::ublas;
36 
37  typedef permutation_matrix<std::size_t> pmatrix;
38 
39  // create a working copy of the input
40  M A(input);
41  // create a permutation matrix for the LU-factorization
42  pmatrix pm(A.size1());
43 
44  // perform LU-factorization
45  int res = lu_factorize(A,pm);
46  if( res != 0 ) return false;
47 
48  // create identity matrix of "inverse"
49  inverse.assign(identity_matrix<typename M::value_type>(A.size1()));
50 
51  // backsubstitute to get the inverse
52  lu_substitute(A, pm, inverse);
53 
54  return true;
55 }
56 
57 
58 /**
59 * Invert a matrix via gauss-jordan algorithm (PARTIAL PIVOT)
60 *
61 * @param m The matrix to invert. Must be square.
62 * @param singular If the matrix was found to be singular, then this
63 * is set to true, else set to false.
64 * @return If singular is false, then the inverted matrix is returned.
65 * Otherwise it contains random values.
66 */
67 template<class T>
68 //#define T double /// for debug
69 boost::numeric::ublas::matrix<T>
70 gjinverse(const boost::numeric::ublas::matrix<T> &m,
71  bool &singular)
72 {
73  using namespace boost::numeric::ublas;
74 
75  const size_t size = m.size1();
76 
77  // Cannot invert if non-square matrix or 0x0 matrix.
78  // Report it as singular in these cases, and return
79  // a 0x0 matrix.
80  if (size != m.size2() || size == 0)
81  {
82  singular = true;
83  matrix<T> A(0,0);
84  return A;
85  }
86 
87  // Handle 1x1 matrix edge case as general purpose
88  // inverter below requires 2x2 to function properly.
89  if (size == 1)
90  {
91  matrix<T> A(1, 1);
92  if (m(0,0) == 0.0)
93  {
94  singular = true;
95  return A;
96  }
97  singular = false;
98  A(0,0) = 1/m(0,0);
99  return A;
100  }
101 
102  // Create an augmented matrix A to invert. Assign the
103  // matrix to be inverted to the left hand side and an
104  // identity matrix to the right hand side.
105  matrix<T> A(size, 2*size);
106  matrix_range<matrix<T> > Aleft(A,
107  range(0, size),
108  range(0, size));
109  Aleft = m;
110  matrix_range<matrix<T> > Aright(A,
111  range(0, size),
112  range(size, 2*size));
113  Aright = identity_matrix<T>(size);
114 
115  // Doing partial pivot
116  for (size_t k = 0; k < size; k++)
117  {
118  // Swap rows to eliminate zero diagonal elements.
119  for (size_t kk = 0; kk < size; kk++)
120  {
121  if ( A(kk,kk) == 0 ) // XXX: test for "small" instead
122  {
123  // Find a row(l) to swap with row(k)
124  int l = -1;
125  for (size_t i = kk+1; i < size; i++)
126  {
127  if ( A(i,kk) != 0 )
128  {
129  l = i;
130  break;
131  }
132  }
133 
134  // Swap the rows if found
135  if ( l < 0 )
136  {
137  std::cerr << "Error:" << __FUNCTION__ << ":"
138  << "Input matrix is singular, because cannot find"
139  << " a row to swap while eliminating zero-diagonal.";
140  singular = true;
141  return Aleft;
142  }
143  else
144  {
145  matrix_row<matrix<T> > rowk(A, kk);
146  matrix_row<matrix<T> > rowl(A, l);
147  rowk.swap(rowl);
148 
149 /*#if defined(DEBUG) || !defined(NDEBUG)
150  std::cerr << __FUNCTION__ << ":"
151  << "Swapped row " << kk << " with row " << l
152  << ":" << A << "\n";
153 #endif*/
154  }
155  }
156  }
157 
158  /////////////////////////////////////////////////////////////////////////////////////////////////////////
159  // normalize the current row
160  for (size_t j = k+1; j < 2*size; j++)
161  A(k,j) /= A(k,k);
162  A(k,k) = 1;
163 
164  // normalize other rows
165  for (size_t i = 0; i < size; i++)
166  {
167  if ( i != k ) // other rows // FIX: PROBLEM HERE
168  {
169  if ( A(i,k) != 0 )
170  {
171  for (size_t j = k+1; j < 2*size; j++)
172  A(i,j) -= A(k,j) * A(i,k);
173  A(i,k) = 0;
174  }
175  }
176  }
177 
178 /*#if defined(DEBUG) || !defined(NDEBUG)
179  std::cerr << __FUNCTION__ << ":"
180  << "GJ row " << k << " : " << A << "\n";
181 #endif*/
182  }
183 
184  singular = false;
185  return Aright;
186 }
187 
188 #endif // _MATRIXINVERSE_HPP_