lu.h

Go to the documentation of this file.

// -*- c-basic-offset: 4 -*-

/* The LU decomposition function was taken from the rocketworkbench project:
 * 
 * lu.c  -  PA = LU factorisation with pivoting
 * $Id$
 * Copyright (C) 2000
 *    Antoine Lefebvre <antoine.lefebvre@polymtl.ca>
 *
 *
 * Licensed under the GPLv2
 */


#ifndef _HUGIN_MATH_LU_H
#define _HUGIN_MATH_LU_H


/* NOTE on matrix representation
 * -----------------------------
 * All matrix should be allocate the following way
 *
 * matrix = (double *) malloc (sizeof(double) * line * column)
 *
 * to access the value at line 2, column 4, you do it like that
 *
 * matrix[2 + 4*line]
 */


/* Find the solution of linear system of equation using the
 * LU factorisation method as explain in
 * 'Advanced engineering mathematics' bye Erwin Kreyszig.
 *
 * This algorithm will also do column permutation to find
 * the larger pivot.
 *
 * ARGUMENTS
 * ---------
 * matrix: the augmented matrix of coefficient in the system
 *         with right hand side value.
 *
 * solution: the solution vector
 *
 * neq: number of equation in the system
 *
 * Antoine Lefebvre
 *    february 6, 2000 Initial version
 *    october 20, 2000 revision of the permutation method
 */

extern "C"
{
int math_lu_solve(double *matrix, double *solution, int neq);
}


namespace hugin_utils
{

    class LMS_Solver
    {
    public:
        LMS_Solver(unsigned nEq)
        {
            m_nEq = nEq;
            m_AtA = new double[nEq*(nEq+1)];
            for (unsigned i=0; i < nEq*(nEq+1); i++) m_AtA[i] = 0;
        }

        ~LMS_Solver()
        {
            delete[] m_AtA;
        }

        template <class Iter>
        void addRow(Iter Arow, double b)
        {
            for( unsigned i=0; i<m_nEq; ++i)
            {
                // calculate  Atb
                m_AtA[i + m_nEq*m_nEq]+=Arow[i]*b;
                for( unsigned j=0; j<m_nEq; ++j)
                {
                    m_AtA[i + j*m_nEq] += Arow[i]*Arow[j];
                }
            }
        }

        template <class Vector>
        bool solve(Vector & x)
        {
            double * solution = new double[m_nEq];
            bool ret = math_lu_solve(m_AtA, solution, m_nEq) != 0;
            for (unsigned i=0; i < m_nEq; i++) {
                x[i] = solution[i];
            }
            delete[] solution;
            return ret;
        }

    protected:
        unsigned m_nEq;
        // matrix that contains [AtA  Atb]
        // uses fortran array access conventions
        double * m_AtA;
    };

} // namespace

#endif // _H

---