block_solver.hpp

// 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.
 
#include "sparse_optimizer.h"
#include <Eigen/LU>
#include "macros.h"
#include "misc.h"
 
namespace NDEVR 
{
 
using namespace Eigen;
 
template <typename Traits>
BlockSolver<Traits>::BlockSolver() 
    : Solver()
{
    _numPoses=0;
    _numLandmarks=0;
    _sizePoses=0;
    _sizeLandmarks=0;
    _doSchur=true;
}
 
template <typename Traits>
void BlockSolver<Traits>::resize(IndexScratch& scratch, int s)
{
    
    resizeVector(s);
 
    if (_doSchur) {
        // the following two are only used in schur
        assert(_sizePoses > 0 && "allocating with wrong size");
        _coefficients.setSize(s);
        _bschur.setSize(_sizePoses);
    }
 
    _Hpp = PoseHessianType(scratch.block_pose_indices.begin(), scratch.block_pose_indices.begin(), scratch.block_pose_indices.size(), scratch.block_pose_indices.size());
    if (_doSchur) {
        _Hschur = PoseHessianType(scratch.block_pose_indices.begin(), scratch.block_pose_indices.begin(), scratch.block_pose_indices.size(), scratch.block_pose_indices.size());
        _Hll = LandmarkHessianType(scratch.block_landmark_indices.begin(), scratch.block_landmark_indices.begin(), scratch.block_landmark_indices.size(), scratch.block_landmark_indices.size());
        _DInvSchur = SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll.colBlockIndices());
        _Hpl = PoseLandmarkHessianType(scratch.block_pose_indices.begin(), scratch.block_landmark_indices.begin(), scratch.block_pose_indices.size(), scratch.block_landmark_indices.size());
        _HplCCS = SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl.rowBlockIndices(), _Hpl.colBlockIndices());
        _HschurTransposedCCS = SparseBlockMatrixCCS<PoseMatrixType>(_Hschur.colBlockIndices(), _Hschur.rowBlockIndices());
#ifdef G2O_OPENMP
        _coefficientsMutex.resize(numPoseBlocks);
#endif
    }
}
 
template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks, IndexScratch& scratch)
{
    uint04 sparseDim = 0;
    scratch.block_pose_indices.clear();
    scratch.block_landmark_indices.clear();
    _numPoses = 0;
    _numLandmarks = 0;
    _sizePoses = 0;
    _sizeLandmarks = 0;
    const uint04 size = solver.optimizer.indexMapping().size();
    for (uint04 i = 0; i < size; ++i)
    {
        OptimizableGraph::OGVertex* v = solver.optimizer.indexMapping()[i];
        int dim = v->dimension();
        if (!v->marginalized())
        {
            v->setColInHessian(_sizePoses);
            _sizePoses += dim;
            scratch.block_pose_indices.add(_sizePoses);
            ++_numPoses;
        }
        else
        {
            v->setColInHessian(_sizeLandmarks);
            _sizeLandmarks += dim;
            scratch.block_landmark_indices.add(_sizeLandmarks);
            ++_numLandmarks;
        }
        sparseDim += dim;
    }
    resize(scratch, sparseDim);
 
    // allocate the diagonal on Hpp and Hll
    uint04 poseIdx = 0;
    uint04 landmarkIdx = 0;
    for (uint04 i = 0; i < solver.optimizer.indexMapping().size(); ++i)
    {
        OptimizableGraph::OGVertex* v = solver.optimizer.indexMapping()[i];
        if (!v->marginalized()) 
        {
            //assert(poseIdx == v.hessianIndex());
            PoseMatrixType* m = _Hpp.block(poseIdx, poseIdx, true);
            if (zeroBlocks)
                m->setZero();
            v->mapHessianMemory(m->data());
            ++poseIdx;
        }
        else
        {
            LandmarkMatrixType* m = _Hll.block(landmarkIdx, landmarkIdx, true);
            if (zeroBlocks)
                m->setZero();
            v->mapHessianMemory(m->data());
            ++landmarkIdx;
        }
    }
    assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
 
    // temporary structures for building the pattern of the Schur complement
    SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
    if (_doSchur) {
        schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur.rowBlockIndices(), _Hschur.colBlockIndices());
        schurMatrixLookup->columns().setSize(_Hschur.blockCols().size());
    }
 
    // here we assume that the landmark indices start after the pose ones
    // create the structure in Hpp, Hll and in Hpl
    for (uint04 i = 0; i < solver.optimizer.activeEdges().size(); i++)
    {
        OptimizableGraph::OGEdge* e = solver.optimizer.activeEdges()[i];
 
        for (uint04 viIdx = 0; viIdx < e->vertexCount(); ++viIdx)
        {
            OptimizableGraph::OGVertex* v1 = (OptimizableGraph::OGVertex*)e->vertex(viIdx);
            int ind1 = v1->hessianIndex();
            if (ind1 == -1)
                continue;
            int indexV1Bak = ind1;
            for (uint04 vjIdx = viIdx + 1; vjIdx < e->vertexCount(); ++vjIdx)
            {
                OptimizableGraph::OGVertex* v2 = (OptimizableGraph::OGVertex*)e->vertex(vjIdx);
                int ind2 = v2->hessianIndex();
                if (ind2 == -1)
                    continue;
                ind1 = indexV1Bak;
                bool transposedBlock = ind1 > ind2;
                if (transposedBlock) // make sure, we allocate the upper triangle block
                    std::swap(ind1, ind2);
                if (!v1->marginalized() && !v2->marginalized()) {
                    PoseMatrixType* m = _Hpp.block(ind1, ind2, true);
                    if (zeroBlocks)
                        m->setZero();
                    e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
                    if (_Hschur.cols() > 0) {// assume this is only needed in case we solve with the schur complement
                        schurMatrixLookup->addBlock(ind1, ind2);
                    }
                }
                else if (v1->marginalized() && v2->marginalized()) {
                    // RAINER hmm.... should we ever reach this here????
                    LandmarkMatrixType* m = _Hll.block(ind1 - _numPoses, ind2 - _numPoses, true);
                    if (zeroBlocks)
                        m->setZero();
                    e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
                }
                else 
                {
                    if (v1->marginalized()) 
                    {
                        PoseLandmarkMatrixType* m = _Hpl.block(v2->hessianIndex(), v1->hessianIndex() - _numPoses, true);
                        if (zeroBlocks)
                            m->setZero();
                        e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
                    }
                    else
                    {
                        PoseLandmarkMatrixType* m = _Hpl.block(v1->hessianIndex(), v2->hessianIndex() - _numPoses, true);
                        if (zeroBlocks)
                            m->setZero();
                        e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
                    }
                }
            }
        }
    }
 
    if (!_doSchur)
        return true;
 
    _DInvSchur.diagonal().resize(landmarkIdx);
    _Hpl.fillSparseBlockMatrixCCS(_HplCCS);
 
    for (uint04 i = 0; i < solver.optimizer.indexMapping().size(); ++i) {
        OptimizableGraph::OGVertex* v = solver.optimizer.indexMapping()[i];
        if (v->marginalized())
        {
            const Set<HyperGraph::HGEdge*>& vedges = v->edges();
            for (auto it1 = vedges.begin(); it1 != vedges.end(); ++it1)
            {
                for (uint04 i = 0; i < (*it1)->vertexCount(); ++i)
                {
                    OptimizableGraph::OGVertex* v1 = (OptimizableGraph::OGVertex*)(*it1)->vertex(i);
                    if (v1->hessianIndex() == -1 || v1 == v)
                        continue;
                    for (auto it2 = vedges.begin(); it2 != vedges.end(); ++it2) {
                        for (uint04 j = 0; j < (*it2)->vertexCount(); ++j)
                        {
                            OptimizableGraph::OGVertex* v2 = (OptimizableGraph::OGVertex*)(*it2)->vertex(j);
                            if (v2->hessianIndex() == -1 || v2 == v)
                                continue;
                            int i1 = v1->hessianIndex();
                            int i2 = v2->hessianIndex();
                            if (i1 <= i2) {
                                schurMatrixLookup->addBlock(i1, i2);
                            }
                        }
                    }
                }
            }
        }
    }
 
    _Hschur.takePatternFromHash(*schurMatrixLookup);
    delete schurMatrixLookup;
    _Hschur.fillSparseBlockMatrixCCSTransposed(_HschurTransposedCCS);
 
    return true;
}
 
template <typename Traits>
bool BlockSolver<Traits>::updateStructure(const Buffer<HyperGraph::HGVertex*>& vset, const Set<HyperGraph::HGVertex*>& edges)
{
    for (auto vit : vset) 
    {
        OptimizableGraph::OGVertex* v = static_cast<OptimizableGraph::OGVertex*>(vit);
        int dim = v->dimension();
        if (! v->marginalized())
        {
            v->setColInHessian(_sizePoses);
            _sizePoses+=dim;
            _Hpp.rowBlockIndices().add(_sizePoses);
            _Hpp.colBlockIndices().add(_sizePoses);
            _Hpp.blockCols().add(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
            ++_numPoses;
            int ind = v->hessianIndex();
            PoseMatrixType* m = _Hpp.block(ind, ind, true);
            v->mapHessianMemory(m->data());
        } 
        else 
        {
            //std::cerr << "updateStructure(): Schur not supported" << std::endl;
            abort();
        }
    }
    resizeVector(_sizePoses + _sizeLandmarks);
 
    for (auto it = edges.begin(); it != edges.end(); ++it) 
    {
        OptimizableGraph::OGEdge* e = static_cast<OptimizableGraph::OGEdge*>(*it);
 
        for (uint04 viIdx = 0; viIdx < e->vertexCount(); ++viIdx) 
        {
            OptimizableGraph::OGVertex* v1 = (OptimizableGraph::OGVertex*) e->vertex(viIdx);
            int ind1 = v1->hessianIndex();
            int indexV1Bak = ind1;
            if (ind1 == -1)
                continue;
            for (uint04 vjIdx = viIdx + 1; vjIdx < e->vertexCount(); ++vjIdx) 
            {
                OptimizableGraph::OGVertex* v2 = (OptimizableGraph::OGVertex*) e->vertex(vjIdx);
                int ind2 = v2->hessianIndex();
                if (ind2 == -1)
                    continue;
                ind1 = indexV1Bak;
                bool transposedBlock = ind1 > ind2;
                if (transposedBlock) // make sure, we allocate the upper triangular block
                    std::swap(ind1, ind2);
 
                if (! v1->marginalized() && !v2->marginalized()) 
                {
                    PoseMatrixType* m = _Hpp.block(ind1, ind2, true);
                    e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
                } else { 
                    //std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
                }
            }
        }
 
    }
 
    return true;
}
 
 
/*
template <typename Traits>
bool BlockSolver<Traits>::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const Buffer<std::pair<int, int> >& blockIndices)
{
    bool ok = solver.solvePattern(spinv, blockIndices, *_Hpp);
    
    return ok;
}*/
 
template <typename Traits>
bool BlockSolver<Traits>::setLambda(g_type lambda, bool backup)
{
    if (backup) {
        _diagonalBackupPose.resize(_numPoses);
        _diagonalBackupLandmark.resize(_numLandmarks);
    }
# ifdef G2O_OPENMP
//# pragma omp parallel for default (shared) if (_numPoses > 100)
# endif
    for (uint04 i = 0; i < _numPoses; ++i) 
    {
        PoseMatrixType *b=_Hpp.block(i,i);
        if (backup)
            _diagonalBackupPose[i] = b->diagonal();
        b->diagonal().array() += lambda;
    }
# ifdef G2O_OPENMP
//# pragma omp parallel for default (shared) if (_numLandmarks > 100)
# endif
    for (uint04 i = 0; i < _numLandmarks; ++i) 
    {
        LandmarkMatrixType *b=_Hll.block(i,i);
        if (backup)
            _diagonalBackupLandmark[i] = b->diagonal();
        b->diagonal().array() += lambda;
    }
    return true;
}
 
template <typename Traits>
void BlockSolver<Traits>::restoreDiagonal()
{
    lib_assert(_diagonalBackupPose.size() == _numPoses, "Mismatch in dimensions");
    lib_assert(_diagonalBackupLandmark.size() == _numLandmarks, "Mismatch in dimensions");
    for (uint04 i = 0; i < _numPoses; ++i) {
        PoseMatrixType *b=_Hpp.block(i,i);
        b->diagonal() = _diagonalBackupPose[i];
    }
    for (uint04 i = 0; i < _numLandmarks; ++i) {
        LandmarkMatrixType *b=_Hll.block(i,i);
        b->diagonal() = _diagonalBackupLandmark[i];
    }
}
 
template <typename Traits>
bool BlockSolver<Traits>::init(bool online)
{
    if (! online) {
        _Hpp.clear();
        _Hpl.clear();
        _Hll.clear();
    }
    solver.init();
    return true;
}
 
} // end namespace