TooN: gaussian_elimination.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

gaussian_elimination.h

    00001 // -*- c++ -*-
    00002
    00003 // Copyright (C) 2008,2009 Ethan Eade, Tom Drummond (twd20@cam.ac.uk)
    00004 // and Ed Rosten (er258@cam.ac.uk)
    00005 //
    00006 // This file is part of the TooN Library.  This library is free
    00007 // software; you can redistribute it and/or modify it under the
    00008 // terms of the GNU General Public License as published by the
    00009 // Free Software Foundation; either version 2, or (at your option)
    00010 // any later version.
    00011
    00012 // This library is distributed in the hope that it will be useful,
    00013 // but WITHOUT ANY WARRANTY; without even the implied warranty of
    00014 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    00015 // GNU General Public License for more details.
    00016
    00017 // You should have received a copy of the GNU General Public License along
    00018 // with this library; see the file COPYING.  If not, write to the Free
    00019 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
    00020 // USA.
    00021
    00022 // As a special exception, you may use this file as part of a free software
    00023 // library without restriction.  Specifically, if other files instantiate
    00024 // templates or use macros or inline functions from this file, or you compile
    00025 // this file and link it with other files to produce an executable, this
    00026 // file does not by itself cause the resulting executable to be covered by
    00027 // the GNU General Public License.  This exception does not however
    00028 // invalidate any other reasons why the executable file might be covered by
    00029 // the GNU General Public License.
    00030
    00031
    00032 #ifndef GAUSSIAN_ELIMINATION_H
    00033 #define GAUSSIAN_ELIMINATION_H
    00034
    00035 #include <utility>
    00036 #include <cmath>
    00037 #include <TooN/TooN.h>
    00038
    00039 namespace TooN {
    00040     ///@ingroup gEquations
    00041     ///Return the solution for \f$Ax = b\f$, given \f$A\f$ and \f$b\f$
    00042     ///@param A \f$A\f$
    00043     ///@param b \f$b\f$
    00044     template<int N, typename Precision>
    00045     inline Vector<N, Precision> gaussian_elimination (Matrix<N,N,Precision> A, Vector<N, Precision> b) {
    00046         using std::swap;
    00047         using std::abs;
    00048
    00049         int size=b.size();
    00050
    00051         for (int i=0; i<size; ++i) {
    00052             int argmax = i;
    00053             Precision maxval = abs(A[i][i]);
    00054
    00055             for (int ii=i+1; ii<size; ++ii) {
    00056                 double v =  abs(A[ii][i]);
    00057                 if (v > maxval) {
    00058                     maxval = v;
    00059                     argmax = ii;
    00060                 }
    00061             }
    00062             Precision pivot = A[argmax][i];
    00063             //assert(abs(pivot) > 1e-16);
    00064             Precision inv_pivot = static_cast<Precision>(1)/pivot;
    00065             if (argmax != i) {
    00066                 for (int j=i; j<size; ++j)
    00067                     swap(A[i][j], A[argmax][j]);
    00068                 swap(b[i], b[argmax]);
    00069             }
    00070             //A[i][i] = 1;
    00071             for (int j=i+1; j<size; ++j)
    00072                 A[i][j] *= inv_pivot;
    00073             b[i] *= inv_pivot;
    00074
    00075             for (int u=i+1; u<size; ++u) {
    00076                 double factor = A[u][i];
    00077                 //A[u][i] = 0;
    00078                 for (int j=i+1; j<size; ++j)
    00079                     A[u][j] -= factor * A[i][j];
    00080                 b[u] -= factor * b[i];
    00081             }
    00082         }
    00083
    00084         Vector<N,Precision> x(size);
    00085         for (int i=size-1; i>=0; --i) {
    00086             x[i] = b[i];
    00087             for (int j=i+1; j<size; ++j)
    00088                 x[i] -= A[i][j] * x[j];
    00089         }
    00090         return x;
    00091     }
    00092
    00093     namespace Internal
    00094     {
    00095         template<int i, int j, int k> struct Size3
    00096         {
    00097             static const int s=(i!= -1)?i:(j!=-1?j:k);
    00098         };
    00099
    00100     };
    00101
    00102     ///@ingroup gEquations
    00103     ///Return the solution for \f$Ax = b\f$, given \f$A\f$ and \f$b\f$
    00104     ///@param A \f$A\f$
    00105     ///@param b \f$b\f$
    00106     template<int R1, int C1, int R2, int C2, typename Precision>
    00107     inline Matrix<Internal::Size3<R1, C1, R2>::s, C2, Precision> gaussian_elimination (Matrix<R1,C1,Precision> A, Matrix<R2, C2, Precision> b) {
    00108         using std::swap;
    00109         using std::abs;
    00110         SizeMismatch<R1, C1>::test(A.num_rows(), A.num_cols());
    00111         SizeMismatch<R1, R2>::test(A.num_rows(), b.num_rows());
    00112
    00113         int size=A.num_rows();
    00114
    00115         for (int i=0; i<size; ++i) {
    00116             int argmax = i;
    00117             Precision maxval = abs(A[i][i]);
    00118
    00119             for (int ii=i+1; ii<size; ++ii) {
    00120                 double v =  abs(A[ii][i]);
    00121                 if (v > maxval) {
    00122                     maxval = v;
    00123                     argmax = ii;
    00124                 }
    00125             }
    00126             Precision pivot = A[argmax][i];
    00127             //assert(abs(pivot) > 1e-16);
    00128             Precision inv_pivot = static_cast<Precision>(1)/pivot;
    00129             if (argmax != i) {
    00130                 for (int j=i; j<size; ++j)
    00131                     swap(A[i][j], A[argmax][j]);
    00132
    00133                 for(int j=0; j < b.num_cols(); j++)
    00134                     swap(b[i][j], b[argmax][j]);
    00135             }
    00136             //A[i][i] = 1;
    00137             for (int j=i+1; j<size; ++j)
    00138                 A[i][j] *= inv_pivot;
    00139             b[i] *= inv_pivot;
    00140
    00141             for (int u=i+1; u<size; ++u) {
    00142                 double factor = A[u][i];
    00143                 //A[u][i] = 0;
    00144                 for (int j=i+1; j<size; ++j)
    00145                     A[u][j] -= factor * A[i][j];
    00146                 b[u] -= factor * b[i];
    00147             }
    00148         }
    00149
    00150         Matrix<Internal::Size3<R1, C1, R2>::s,C2,Precision> x(b.num_rows(), b.num_cols());
    00151         for (int i=size-1; i>=0; --i) {
    00152             for(int k=0; k <b.num_cols(); k++)
    00153             {
    00154                 x[i][k] = b[i][k];
    00155                 for (int j=i+1; j<size; ++j)
    00156                     x[i][k] -= A[i][j] * x[j][k];
    00157             }
    00158         }
    00159         return x;
    00160     }
    00161 }
    00162 #endif