linear_solver_eigen.h

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
#ifndef G2O_LINEAR_SOLVER_EIGEN_H
#define G2O_LINEAR_SOLVER_EIGEN_H
 
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
 
#include "linear_solver.h"
 
#include "eigen_types.h"
 
namespace NDEVR {

    template <typename MatrixType>
    class LinearSolverEigen
    {
    public:
        SparseOptimizer optimizer;  
        typedef Eigen::SparseMatrix<g_type, Eigen::ColMajor> SparseMatrix;  
        typedef Eigen::Triplet<g_type> Triplet;                             
        typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> PermutationMatrix;    
        class CholeskyDecomposition : public Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>
        {
        public:
            CholeskyDecomposition() : Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>() {}
            using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;

            void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation)
            {
                m_Pinv = permutation;
                m_P = permutation.inverse();
                int size = cast<int>(a.cols());
                SparseMatrix ap(size, size);
                ap.selfadjointView<Eigen::Upper>() = a.selfadjointView<UpLo>().twistedBy(m_P);
                analyzePattern_preordered(ap, true);
            }
        };
 
    public:
        LinearSolverEigen()
            : _init(true)
            , _blockOrdering(false)
        {
        }
 
        ~LinearSolverEigen()
        {
        }

        virtual bool init()
        {
            _init = true;
            return true;
        }

        bool solve(const SparseBlockMatrix<MatrixType>& A, g_type* x, g_type* b)
        {
            if (_init)
                _sparseMatrix.resize(A.rows(), A.cols());
            fillSparseMatrix(A, !_init);
            if (_init) // compute the symbolic composition once
                computeSymbolicDecomposition(A);
            _init = false;
 
            _cholesky.factorize(_sparseMatrix);
            if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite
                return false;
            }
 
            // Solving the system
            Eigen::VectorX<g_type>::MapType xx(x, _sparseMatrix.cols());
            Eigen::VectorX<g_type>::ConstMapType bb(b, _sparseMatrix.cols());
            xx = _cholesky.solve(bb);
 
            return true;
        }

        bool blockOrdering() const { return _blockOrdering; }
        void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering; }
 
 
    protected:
        bool _init;                             
        bool _blockOrdering;                    
        SparseMatrix _sparseMatrix;             
        CholeskyDecomposition _cholesky;            

        void computeSymbolicDecomposition(const SparseBlockMatrix<MatrixType>& A)
        {
            if (!_blockOrdering) {
                _cholesky.analyzePattern(_sparseMatrix);
            }
            else {
                // block ordering with the Eigen Interface
                // This is really ugly currently, as it calls internal functions from Eigen
                // and modifies the SparseMatrix class
                Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> blockP;
                {
                    // prepare a block structure matrix for calling AMD
                    std::vector<Triplet> triplets;
                    for (uint04 c = 0; c < A.blockCols().size(); ++c) {
                        const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
                        for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
                            const int& r = it->first;
                            if (r > static_cast<int>(c)) // only upper triangle
                                break;
                            triplets.push_back(Triplet(r, c, 0.));
                        }
                    }
 
                    // call the AMD ordering on the block matrix.
                    // Relies on Eigen's internal stuff, probably bad idea
                    SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size());
                    auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());
                    typename CholeskyDecomposition::CholMatrixType C;
                    C = auxBlockMatrix.selfadjointView<Eigen::Upper>();
                    Eigen::internal::minimum_degree_ordering(C, blockP);
                }
 
                int rows = A.rows();
                assert(rows == A.cols() && "Matrix A is not square");
 
                // Adapt the block permutation to the scalar matrix
                PermutationMatrix scalarP;
                scalarP.resize(rows);
                int scalarIdx = 0;
                for (int i = 0; i < blockP.size(); ++i) {
                    const int& p = blockP.indices()(i);
                    int base = A.colBaseOfBlock(p);
                    int nCols = A.colsOfBlock(p);
                    for (int j = 0; j < nCols; ++j)
                        scalarP.indices()(scalarIdx++) = base++;
                }
                assert(scalarIdx == rows && "did not completely fill the permutation matrix");
                // analyze with the scalar permutation
                _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP);
 
            }
        }
 
        void fillSparseMatrix(const SparseBlockMatrix<MatrixType>& A, bool onlyValues)
        {
            if (onlyValues) 
            {
                A.fillCCS(_sparseMatrix.valuePtr(), true);
            }
            else 
            {
 
                // create from triplet structure
                std::vector<Triplet> triplets;
                triplets.reserve(A.nonZeros());
                for (uint04 c = 0; c < A.blockCols().size(); ++c) {
                    int colBaseOfBlock = A.colBaseOfBlock(c);
                    const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
                    for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
                        int rowBaseOfBlock = A.rowBaseOfBlock(it->first);
                        const MatrixType& m = it->second;
                        for (int cc = 0; cc < m.cols(); ++cc) {
                            int aux_c = colBaseOfBlock + cc;
                            for (int rr = 0; rr < m.rows(); ++rr) {
                                int aux_r = rowBaseOfBlock + rr;
                                if (aux_r > aux_c)
                                    break;
                                triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc)));
                            }
                        }
                    }
                }
                _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());
 
            }
        }
    };
 
} // end namespace
 
#endif