ProteoWizard
MatrixInverse.hpp
Go to the documentation of this file.
1//
2// $Id$
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.
30Uses lu_factorize and lu_substitute in uBLAS to invert a matrix */
31template<typename M>
32bool 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*/
67template<class T>
68//#define T double /// for debug
69boost::numeric::ublas::matrix<T>
70gjinverse(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_
#define A
Kernel k
bool InvertMatrix(const M &input, M &inverse)
boost::numeric::ublas::matrix< T > gjinverse(const boost::numeric::ublas::matrix< T > &m, bool &singular)
Invert a matrix via gauss-jordan algorithm (PARTIAL PIVOT)