00001 // -*- c++ -*- 00002 00003 // Copyright (C) 2009 Ed Rosten (er258@cam.ac.uk) 00004 // 00005 // This file is part of the TooN Library. This library is free 00006 // software; you can redistribute it and/or modify it under the 00007 // terms of the GNU General Public License as published by the 00008 // Free Software Foundation; either version 2, or (at your option) 00009 // any later version. 00010 00011 // This library is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU General Public License for more details. 00015 00016 // You should have received a copy of the GNU General Public License along 00017 // with this library; see the file COPYING. If not, write to the Free 00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307, 00019 // USA. 00020 00021 // As a special exception, you may use this file as part of a free software 00022 // library without restriction. Specifically, if other files instantiate 00023 // templates or use macros or inline functions from this file, or you compile 00024 // this file and link it with other files to produce an executable, this 00025 // file does not by itself cause the resulting executable to be covered by 00026 // the GNU General Public License. This exception does not however 00027 // invalidate any other reasons why the executable file might be covered by 00028 // the GNU General Public License. 00029 00030 00031 #ifndef TOON_INC_GAUSS_JORDAN_H 00032 #define TOON_INC_GAUSS_JORDAN_H 00033 00034 #include <utility> 00035 #include <TooN/TooN.h> 00036 00037 namespace TooN 00038 { 00049 template<int R, int C, class Precision, class Base> void gauss_jordan(Matrix<R, C, Precision, Base>& m) 00050 { 00051 using std::swap; 00052 00053 //Loop over columns to reduce. 00054 for(int col=0; col < m.num_rows(); col++) 00055 { 00056 //Reduce the current column to a single element 00057 00058 00059 //Search down the current column in the lower triangle for the largest 00060 //absolute element (pivot). Then swap the pivot row, so that the pivot 00061 //element is on the diagonal. The benchmarks show that it is actually 00062 //faster to swap whole rows than it is to access the rows via indirection 00063 //and swap the indirection element. This holds for both pointer indirection 00064 //and using a permutation vector over rows. 00065 { 00066 int pivotpos = col; 00067 double pivotval = abs(m[pivotpos][col]); 00068 for(int p=col+1; p <m.num_rows(); p++) 00069 if(abs(m[p][col]) > pivotval) 00070 { 00071 pivotpos = p; 00072 pivotval = abs(m[pivotpos][col]); 00073 } 00074 00075 if(col != pivotpos) 00076 for(int c=0; c < m.num_cols(); c++) 00077 swap(m[col][c], m[pivotpos][c]); 00078 } 00079 00080 //Reduce the current column in every row to zero, excluding elements on 00081 //the leading diagonal. 00082 for(int row = 0; row < m.num_rows(); row++) 00083 { 00084 if(row != col) 00085 { 00086 double multiple = m[row][col] / m[col][col]; 00087 00088 //We could eliminate some of the computations in the augmented 00089 //matrix, if the augmented half is the identity. In general, it 00090 //is not. 00091 00092 //Subtract the pivot row from all other rows, to make 00093 //column col zero. 00094 m[row][col] = 0; 00095 for(int c=col+1; c < m.num_cols(); c++) 00096 m[row][c] = m[row][c] - m[col][c] * multiple; 00097 } 00098 } 00099 } 00100 00101 //Final pass to make diagonal elements one. Performing this in a final 00102 //pass allows us to avoid any significant computations on the left-hand 00103 //square matrix, since it is diagonal, and ends up as the identity. 00104 for(int row=0;row < m.num_rows(); row++) 00105 { 00106 double mul = 1/m[row][row]; 00107 00108 m[row][row] = 1; 00109 00110 for(int col=m.num_rows(); col < m.num_cols(); col++) 00111 m[row][col] *= mul; 00112 } 00113 } 00114 00115 } 00116 #endif