00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef GAUSSIAN_ELIMINATION_H
00033 #define GAUSSIAN_ELIMINATION_H
00034
00035 #include <utility>
00036 #include <TooN/TooN.h>
00037
00038 namespace TooN {
00040 template<int N, typename Precision>
00041 inline Vector<N, Precision> gaussian_elimination (Matrix<N,N,Precision> A, Vector<N, Precision> b) {
00042 using std::swap;
00043
00044 int size=b.size();
00045
00046 for (int i=0; i<size; ++i) {
00047 int argmax = i;
00048 Precision maxval = abs(A[i][i]);
00049
00050 for (int ii=i+1; ii<size; ++ii) {
00051 double v = abs(A[ii][i]);
00052 if (v > maxval) {
00053 maxval = v;
00054 argmax = ii;
00055 }
00056 }
00057 Precision pivot = A[argmax][i];
00058
00059 Precision inv_pivot = static_cast<Precision>(1)/pivot;
00060 if (argmax != i) {
00061 for (int j=i; j<size; ++j)
00062 swap(A[i][j], A[argmax][j]);
00063 swap(b[i], b[argmax]);
00064 }
00065
00066 for (int j=i+1; j<size; ++j)
00067 A[i][j] *= inv_pivot;
00068 b[i] *= inv_pivot;
00069
00070 for (int u=i+1; u<size; ++u) {
00071 double factor = A[u][i];
00072
00073 for (int j=i+1; j<size; ++j)
00074 A[u][j] -= factor * A[i][j];
00075 b[u] -= factor * b[i];
00076 }
00077 }
00078
00079 Vector<N,Precision> x(size);
00080 for (int i=size-1; i>=0; --i) {
00081 x[i] = b[i];
00082 for (int j=i+1; j<size; ++j)
00083 x[i] -= A[i][j] * x[j];
00084 }
00085 return x;
00086 }
00087
00088 namespace Internal
00089 {
00090 template<int i, int j, int k> struct Size3
00091 {
00092 static const int s=(i!= -1)?i:(j!=-1?j:k);
00093 };
00094
00095 };
00096
00098 template<int R1, int C1, int R2, int C2, typename Precision>
00099 inline Matrix<Internal::Size3<R1, C1, R2>::s, C2, Precision> gaussian_elimination (Matrix<R1,C1,Precision> A, Matrix<R2, C2, Precision> b) {
00100 using std::swap;
00101 SizeMismatch<R1, C1>::test(A.num_rows(), A.num_cols());
00102 SizeMismatch<R1, R2>::test(A.num_rows(), b.num_rows());
00103
00104 int size=A.num_rows();
00105
00106 for (int i=0; i<size; ++i) {
00107 int argmax = i;
00108 Precision maxval = abs(A[i][i]);
00109
00110 for (int ii=i+1; ii<size; ++ii) {
00111 double v = abs(A[ii][i]);
00112 if (v > maxval) {
00113 maxval = v;
00114 argmax = ii;
00115 }
00116 }
00117 Precision pivot = A[argmax][i];
00118
00119 Precision inv_pivot = static_cast<Precision>(1)/pivot;
00120 if (argmax != i) {
00121 for (int j=i; j<size; ++j)
00122 swap(A[i][j], A[argmax][j]);
00123
00124 for(int j=0; j < b.num_cols(); j++)
00125 swap(b[i][j], b[argmax][j]);
00126 }
00127
00128 for (int j=i+1; j<size; ++j)
00129 A[i][j] *= inv_pivot;
00130 b[i] *= inv_pivot;
00131
00132 for (int u=i+1; u<size; ++u) {
00133 double factor = A[u][i];
00134
00135 for (int j=i+1; j<size; ++j)
00136 A[u][j] -= factor * A[i][j];
00137 b[u] -= factor * b[i];
00138 }
00139 }
00140
00141 Matrix<Internal::Size3<R1, C1, R2>::s,C2,Precision> x(b.num_rows(), b.num_cols());
00142 for (int i=size-1; i>=0; --i) {
00143 for(int k=0; k <b.num_cols(); k++)
00144 {
00145 x[i][k] = b[i][k];
00146 for (int j=i+1; j<size; ++j)
00147 x[i][k] -= A[i][j] * x[j][k];
00148 }
00149 }
00150 return x;
00151 }
00152 }
00153 #endif