block_solver.hpp

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
//
// g2o is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// g2o is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include "graph_optimizer_sparse.h"
#include <Eigen/LU>
#include <fstream>
#include <iomanip>

#include "g2o/stuff/timeutil.h"

namespace g2o {
  using namespace std;
  using namespace Eigen;

template <typename Traits>
BlockSolver<Traits>::BlockSolver
(
   SparseOptimizer* optimizer, LinearSolverType* linearSolver
):
   Solver(optimizer), _linearSolver(linearSolver)
{
   // workspace
   _Hpp=0;
   _Hll=0;
   _Hpl=0;
   _Hschur=0;
   _DInvSchur=0;
   _coefficients=0;
   _bschur = 0;
   _xSize=0;
   _numPoses=0;
   _numLandmarks=0;
   _sizePoses=0;
   _sizeLandmarks=0;
   _doSchur=true;
}

/******************************************************************************/

template <typename Traits>
void BlockSolver<Traits>::resize
(
   int* blockPoseIndices, int numPoseBlocks,
   int* blockLandmarkIndices, int numLandmarkBlocks, int s
)
{
   delete[] _coefficients;
   _coefficients = 0;

   // Definida en solver.[h-cpp]. Reserva espacio a los vectores
   // 'x' y 'b'.   (H+lambda*I) * x = -b; size(b) == size(x)
   resizeVector(s);
   delete _Hpp;
   _Hpp=0;
   delete _Hll;
   _Hll=0;
   delete _Hpl;
   _Hpl = 0;
   delete _Hschur;
   _Hschur=0;
   delete _DInvSchur;
   _DInvSchur=0;

   if (_doSchur)
   {
      // TODO the following two are only used in schur, actually too large...
      _coefficients = new double [s];
      _bschur = new double[s];
   }

   // SparseBlockMatrix: Una matriz de bloques dispersa esta compuesta por
   //    bloques densos que siguen un patron de dispersion no aleatorio.
   //    El patron se especifica con una particion de filas y columnas de la
   //    matriz. Los bloques densos se representan por una Eigen::Matrix y se
   //    pueden declarar estatica o dinamicamente. Todos los bloques de una
   //    matriz de bloques estaticos son del mismo tamanio y el tamanio del
   //    r-esimo bloque esta especificado por el argumento de la plantilla.
   //    Para manejar bloques de diferentes tamanios hay que declarar los
   //    bloques de la matriz dinamicos (argumento de plantilla por defecto).
   //
   // SparseBlockMatrix(int *rbi, int *cbi, int rb, int cb, bool hasStorage)
   //    rbi: Vector de enteros. Disposicion en filas de los bloques.
   //         El componente i-esimo contiene la 1a fila del bloque i+1.
   //    cbi: Vector de enteros. Disposicion en cols de los bloques.
   //         El componente i-esimo contiene la 1a col del bloque i+1.
   //    rb:  numero de bloques fila
   //    cb:  numero de bloques columna
   //    hasStorage: true (defecto) si la matriz contiene los bloques.
   //                false si la matriz solo contiene referencias a los bloques,
   //                es decir, la matriz "ve" bloques ue viven en otro lado.


   // PoseHessianType == SparseBlockMatrix<PoseMatrixType>
   // PoseMatrixType == Matrix<double, 6, 6>
   _Hpp = new
      PoseHessianType
      (
         blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks
      );
   if (_doSchur)
   {
      _Hschur = new
         PoseHessianType
         (
            blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks
         );

      // LandmarkHessianType == SparseBlockMatrix<LandmarkMatrixType>
      // LandmarkMatrixType == Matrix<double, 3, 3>
      _Hll = new
         LandmarkHessianType
         (
            blockLandmarkIndices, blockLandmarkIndices,
            numLandmarkBlocks, numLandmarkBlocks
         );

      _DInvSchur = new
         LandmarkHessianType
         (
            blockLandmarkIndices, blockLandmarkIndices,
            numLandmarkBlocks, numLandmarkBlocks
         );

      // PoseLandmarkHessianType == SparseBlockMatrix<PoseLandmarkMatrixType>
      // PoseLandmarkMatrixType == Matrix<double, 6, 3>
      _Hpl=new
         PoseLandmarkHessianType
         (
            blockPoseIndices, blockLandmarkIndices,
            numPoseBlocks, numLandmarkBlocks
         );
   }
}

/******************************************************************************/

template <typename Traits>
BlockSolver<Traits>::~BlockSolver()
{
   delete _linearSolver;
   if (_Hpp)
   {
      delete _Hpp;
      _Hpp=0;
   }
   if (_Hll)
   {
      delete _Hll;
      _Hll=0;
   }
   if (_Hpl)
   {
      delete _Hpl;
      _Hpl = 0;
   }
   if (_Hschur){
      delete _Hschur;
      _Hschur=0;
   }
   if (_DInvSchur){
      delete _DInvSchur;
      _DInvSchur=0;
   }
   if (_coefficients) {
      delete[] _coefficients;
      _coefficients = 0;
   }
   delete[] _bschur;
   _bschur = 0;
}

/******************************************************************************/

template <typename Traits>
bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
{
   assert(_optimizer);

   size_t sparseDim = 0;
   _numPoses=0;
   _numLandmarks=0;
   _sizePoses=0;
   _sizeLandmarks=0;


// Dimensionamiento de las matrices Hpp, Hll, Hpl y otras
// No tienen memoria asignada, solo se ha definido el patron
   // _optimizer->indexMapping() vector con todos los vertices no fijos.
   // Los vertices fijos tienen como indice temporal un -1 y no estan dentro
   // de este vector.
   int blockPoseIndices[_optimizer->indexMapping().size()];
   int blockLandmarkIndices[_optimizer->indexMapping().size()];


   for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      int dim = v->dimension();

      if (! v->marginalized())
      {
         v->setColInHessian(_sizePoses);
         _sizePoses += dim;
         blockPoseIndices[_numPoses] = _sizePoses;
         _numPoses++;
      } else
      {
         v->setColInHessian(_sizeLandmarks);
         _sizeLandmarks += dim;
         blockLandmarkIndices[_numLandmarks] = _sizeLandmarks;
         _numLandmarks++;
      }
      sparseDim += dim;
   }

   resize
   (
      blockPoseIndices, _numPoses, blockLandmarkIndices,
      _numLandmarks, sparseDim
   );


// Asignacion de memoria para los bloques de la diagonal principal y mapeo
// de esos bloques en sus correspondientes vertices.
   // allocate the diagonal on Hpp and Hll
   int poseIdx = 0;
   int landmarkIdx = 0;
   for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      if (! v->marginalized())
      {
         //assert(poseIdx == v->tempIndex());
         // PoseMatrixType == Matrix<double, 6, 6>
         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());
         _DInvSchur->block(landmarkIdx, landmarkIdx, true);
         ++landmarkIdx;
      }
   }
   assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);

// Asignacion de memoria para el resto de los bloques y mapeo
// de esos bloques en sus correspondientes edges.
// Al final, los vertices que no estan relacionados tienen sus correspondientes
// bloques sin asignar

   // here we assume that the landmark indices start after the pose ones
   // create the structure in Hpp, Hll and in Hpl
   for (SparseOptimizer::EdgeContainer::const_iterator
        it  = _optimizer->activeEdges().begin();
        it != _optimizer->activeEdges().end();
        ++it)
   {
      OptimizableGraph::Edge* e = *it;

      for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx)
      {
         OptimizableGraph::Vertex* v1 =
            (OptimizableGraph::Vertex*) e->vertices()[viIdx];

         int ind1 = v1->tempIndex();
         // Si el vertice es fijo se ignora
         if (ind1 == -1)   continue;
         int indexV1Bak = ind1;

         for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx)
         {
            OptimizableGraph::Vertex* v2 =
               (OptimizableGraph::Vertex*) e->vertices()[vjIdx];

            int ind2 = v2->tempIndex();
            // Si el vertice es fijo se ignora
            if (ind2 == -1)   continue;
            ind1 = indexV1Bak;

            // make sure, we allocate the upper triangle block
            bool transposedBlock = ind1 > ind2;
            if(transposedBlock)   swap(ind1, ind2);

            if (! v1->marginalized() && !v2->marginalized())
            // Si los 2 vertices son Poses
            {
               PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
               if (zeroBlocks)   m->setZero();
               e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);

               // assume this is only needed in case
               // we solve with the schur complement
               if (_Hschur)   _Hschur->block(ind1, ind2, true);

            }else if (v1->marginalized() && v2->marginalized())
            // Si los 2 vertices son Landmarks
            // En nuestro caso esto no tiene que ocurrir,
            // si ocurre algo esta mal
            {
               // 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
            // Si una Pose con un Landmark
            {
               if (v1->marginalized())
               // v1 es Landmark
               {
                  PoseLandmarkMatrixType* m =
                     _Hpl->block
                        (v2->tempIndex(),v1->tempIndex()-_numPoses, true);
                  if (zeroBlocks)   m->setZero();

                  // transpose the block before writing to it
                  e->mapHessianMemory(m->data(), viIdx, vjIdx, true);
               }else
               // v1 es Pose
               {
                  PoseLandmarkMatrixType* m =
                     _Hpl->block
                        (v1->tempIndex(),v2->tempIndex()-_numPoses, true);
                  if (zeroBlocks)   m->setZero();

                  // directly the block
                  e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
               }
            }
         }
      }
   }

   if (! _doSchur)   return true;



// Asignacion de memoria para la matriz de Schur
   // allocate the blocks for the schurr complement
   for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      if (v->marginalized())
      {
         const HyperGraph::EdgeSet& vedges = v->edges();
         for (HyperGraph::EdgeSet::const_iterator
              it1  = vedges.begin();
              it1 != vedges.end();
              it1++)
         {
// OSCAR //

            for(size_t i = 0; i < (*it1)->vertices().size(); i++)
            {
               OptimizableGraph::Vertex* v1;
               v1 = (OptimizableGraph::Vertex*) (*it1)->vertices()[i];
               //if (v1 == v || v1->tempIndex() == -1)   continue;
               if(v1->marginalized() || v1->tempIndex() == -1)   continue;

               for(HyperGraph::EdgeSet::const_iterator
                   it2  = vedges.begin();
                   it2 != vedges.end();
                   it2++)
               {
                  for(size_t i = 0; i < (*it1)->vertices().size(); i++)
                  {
                     OptimizableGraph::Vertex* v2;
                     v2 = (OptimizableGraph::Vertex*) (*it2)->vertices()[i];
                     //if (v2 == v || v2->tempIndex() == -1)   continue;
                     if(v2->marginalized() || v2->tempIndex() == -1)   continue;

                     int i1 = v1->tempIndex();
                     int i2 = v2->tempIndex();
                     if (i1 <= i2)   _Hschur->block(i1,i2,true)->setZero();
                  }
               }
            }

// FIN OSCAR //
/*
            OptimizableGraph::Vertex* v1 =
               (OptimizableGraph::Vertex*) (*it1)->vertices()[0];
            if (v1 == v)
               v1 = (OptimizableGraph::Vertex*) (*it1)->vertices()[1];
            if (v1->tempIndex() == -1)   continue;

            for(HyperGraph::EdgeSet::const_iterator
                it2  = vedges.begin();
                it2 != vedges.end();
                it2++)
            {
               OptimizableGraph::Vertex* v2 =
                  (OptimizableGraph::Vertex*) (*it2)->vertices()[0];
               if (v2 == v)
                  v2 = (OptimizableGraph::Vertex*) (*it2)->vertices()[1];
               if (v2->tempIndex() == -1)   continue;

               int i1 = v1->tempIndex();
               int i2 = v2->tempIndex();
               if (i1 <= i2)   _Hschur->block(i1,i2,true)->setZero();
            }
*/
         }
      }
   }
   return true;
}

/******************************************************************************/

// Redimensionamiento y remapeo en memoria para las Hessianas de las Poses
// y, si no se aplica Schur, de los landmarks.

template <typename Traits>
bool BlockSolver<Traits>::updateStructure
(
   const std::vector<HyperGraph::Vertex*>& vset,
   const HyperGraph::EdgeSet& edges
)
{
   for (std::vector<HyperGraph::Vertex*>::const_iterator
        vit = vset.begin();
        vit != vset.end();
        ++vit)
   {
      OptimizableGraph::Vertex* v =
         static_cast<OptimizableGraph::Vertex*>(*vit);
      int dim = v->dimension();
      if (! v->marginalized())
      {
         // Pose
         v->setColInHessian(_sizePoses);
         _sizePoses += dim;
         _Hpp->rowBlockIndices().push_back(_sizePoses);
         _Hpp->colBlockIndices().push_back(_sizePoses);
         _Hpp->blockCols().push_back
         (
            typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap()
         );
         _numPoses++;
         int ind = v->tempIndex();
         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 (HyperGraph::EdgeSet::const_iterator
        it = edges.begin();
        it != edges.end();
        ++it)
   {
      OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);

      for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx)
      {
         OptimizableGraph::Vertex* v1 =
            (OptimizableGraph::Vertex*) e->vertices()[viIdx];

         int ind1 = v1->tempIndex();
         int indexV1Bak = ind1;
         // Si el vertice es fijo se ignora
         if (ind1 == -1)   continue;

         for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx)
         {
            OptimizableGraph::Vertex* v2 =
               (OptimizableGraph::Vertex*) e->vertices()[vjIdx];

            int ind2 = v2->tempIndex();
            // Si el vertice es fijo se ignora
            if (ind2 == -1)   continue;
            ind1 = indexV1Bak;

            bool transposedBlock = ind1 > ind2;
            // make sure, we allocate the upper triangular block
            if (transposedBlock)   swap(ind1, ind2);

            if (! v1->marginalized() && !v2->marginalized())
            {
               // Ambos vertices son Poses
               PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
               e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
            }else
            {
               std::cerr << __PRETTY_FUNCTION__ << ": not supported\n";
            }
         }
      }
   }

   return true;
}

/******************************************************************************/

// Resuelve el sistema H * x = -b
template <typename Traits>
bool BlockSolver<Traits>::solve()
{
   //cerr << __PRETTY_FUNCTION__ << endl;
   if (! _doSchur)
   {
      // Resolucion del sistema completo
      double t=get_time();
      bool ok = _linearSolver->solve(*_Hpp, _x, _b);
      if (globalStats)
      {
         globalStats->timeLinearSolver = get_time() - t;
         globalStats->hessianDimension =
            globalStats->hessianPoseDimension =
            _Hpp->cols();
      }
      return ok;
   }

   char fich[] = "Hpp.m";
   _Hpp->writeOctave(fich, false);
    fich[2] = 'l';
   _Hpl->writeOctave(fich, false);
    fich[1] = 'l';
   _Hll->writeOctave(fich, false);

/*
   cerr << "Pausa. Presiona Enter para continuar.\n";
   char a;
   cin >> a;
*/
   // Resolucion mediante el complemento de Schur
   // schur thing

   // backup the coefficient matrix
   double t=get_time();
   _Hschur->clear();
   _Hpp->add(_Hschur);
   _DInvSchur->clear();
   memset(_coefficients, 0, _xSize*sizeof(double));

   for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      if (v->marginalized())
      {
         // El vertice es un landmark
         int landmarkIndex = i - _numPoses;
         const HyperGraph::EdgeSet& vedges = v->edges();

         const LandmarkMatrixType *D = _Hll->block(landmarkIndex,landmarkIndex);
         assert (D && D->rows()==D->cols());

         LandmarkMatrixType Dinv = D->inverse();
         LandmarkMatrixType *_DInvSchurBlock =
            _DInvSchur->block(landmarkIndex, landmarkIndex, false);
         assert( _DInvSchurBlock
              && _DInvSchurBlock->rows() == D->rows()
              && _DInvSchurBlock->cols() == D->cols());

         *_DInvSchurBlock = Dinv;
         LandmarkVectorType  db(D->rows());
         for (int j=0; j<D->rows(); j++)
         {
            db[j] = _b[v->colInHessian()+_sizePoses+j];
         }

         // db = inv(Hll) * bl
         db = Dinv * db;

         // helper array for OpenMP parallel
         size_t tmpIdx = 0;
#     ifdef _MSC_VER
         OptimizableGraph::Edge* tmpEdges =
            new OptimizableGraph::Edge*[vedges.size()];
#     else
         OptimizableGraph::Edge* tmpEdges[vedges.size()];
#     endif
         for (HyperGraph::EdgeSet::const_iterator
              it2  = vedges.begin();
              it2 != vedges.end();
              ++it2)
         {
            OptimizableGraph::Edge* e2 =
               static_cast<OptimizableGraph::Edge*>(*it2);
            tmpEdges[tmpIdx++] = e2;
         }

#     ifdef G2O_OPENMP
#     pragma omp parallel for default (shared)
#     endif

         for (size_t l=0; l < tmpIdx; ++l)
         {
            OptimizableGraph::Edge* e1 = tmpEdges[l];

// OSCAR //

            for(size_t i = 0; i < e1->vertices().size(); i++)
            {
               OptimizableGraph::Vertex* v1;
               v1 = static_cast<OptimizableGraph::Vertex*>( e1->vertices()[i] );

               if (v1==v)   continue;
               assert (!v1->marginalized());

               int i1 = v1->tempIndex();
               // Si el vertice es fijo lo ignoramos.
               if (i1<0)   continue;

               const PoseLandmarkMatrixType* Bi = _Hpl->block(i1,landmarkIndex);
               assert(Bi);

               // Bd = Hpl * inv(Hll) * bl
               PoseVectorType Bb = (*Bi)*db;

               v1->lockQuadraticForm();
               // coefficients = Hpl * inv(Hll) * bl
               for (int j=0; j<Bb.rows(); j++)
               {
                  _coefficients[v1->colInHessian()+j] += Bb(j);
               }

               // BDinv = Hpl * inv(Hll);
               PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);

               for (size_t k =0; k < tmpIdx; ++k)
               {
                  OptimizableGraph::Edge* e2 = tmpEdges[k];
                  for(size_t i = 0; i < e1->vertices().size(); i++)
                  {
                     OptimizableGraph::Vertex* v2;
                     v2 = (OptimizableGraph::Vertex*) e2->vertices()[i];

                     if(v2==v)  continue;;
                     assert (!v2->marginalized());

                     int i2 = v2->tempIndex();
                     // Si el vertice es fijo o ya esta aniadido por un edge
                     // anterior entonces se ignora
                     if (i2<0 || i1>i2)  continue;

                     const PoseLandmarkMatrixType* Bj =
                        _Hpl->block(i2,landmarkIndex);
                     assert(Bj);

                     PoseMatrixType* Hi1i2 = _Hschur->block(i1,i2);
                     assert(Hi1i2);
                     // HSchur = Hpp - Hpl * inv(Hll) * Hpl'
                     (*Hi1i2).noalias() -= BDinv * Bj->transpose();
                  }
               }
               v1->unlockQuadraticForm();
            }
// FIN OSCAR //

/*
            OptimizableGraph::Vertex* v1 =
               static_cast<OptimizableGraph::Vertex*>( e1->vertices()[0] );

            if (v1==v)   v1 = (OptimizableGraph::Vertex*) e1->vertices()[1];
            assert (!v1->marginalized());

            int i1 = v1->tempIndex();
            // Si el vertice es fijo lo ignoramos
            if (i1<0)   continue;

            const PoseLandmarkMatrixType* Bi = _Hpl->block(i1,landmarkIndex);
            assert(Bi);

            // Bd = Hpl * inv(Hll) * bl
            PoseVectorType Bb = (*Bi)*db;

            v1->lockQuadraticForm();
            // coefficients = Hpl * inv(Hll) * bl
            for (int j=0; j<Bb.rows(); j++)
            {
               _coefficients[v1->colInHessian()+j] += Bb(j);
            }

            // BDinv = Hpl * inv(Hll);
            PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);

            for (size_t k =0; k < tmpIdx; ++k)
            {
               OptimizableGraph::Edge* e2 = tmpEdges[k];
               OptimizableGraph::Vertex* v2 =
                  (OptimizableGraph::Vertex*) e2->vertices()[0];

               if (v2==v)   v2 = (OptimizableGraph::Vertex*) e2->vertices()[1];
               assert (!v2->marginalized());

               int i2 = v2->tempIndex();
               // Si el vertice es fijo o ya esta aniadido por un edge anterior
               // entonces se ignora
               if (i2<0)   continue;
               if (i1>i2)  continue;

               const PoseLandmarkMatrixType* Bj = _Hpl->block(i2,landmarkIndex);
               assert(Bj);

               PoseMatrixType* Hi1i2 = _Hschur->block(i1,i2);
               assert(Hi1i2);
               // HSchur = Hpp - Hpl * inv(Hll) * Hpl'
               (*Hi1i2).noalias() -= BDinv * Bj->transpose();
            }
            v1->unlockQuadraticForm();
*/
         }
#     ifdef _MSC_VER
         delete[] tmpEdges;
#     endif
      }
   }
   //cerr << "Solve [marginalize] = " <<  get_time()-t << endl;



   // bSchur = -bp + Hpl * inv(Hll) * bl
   // _bschur = _b for calling solver, and not touching _b
   memcpy(_bschur, _b, _xSize * sizeof(double));
   for (int i=0; i<_sizePoses; i++)   _bschur[i] -= _coefficients[i];

   if (globalStats)   globalStats->timeSchurrComplement = get_time() - t;
   t = get_time();

   // Resolucion de las poses
   // HSchur = Hpp - Hpl * inv(Hll) * Hpl'
   // bSchur = -bp + Hpl * inv(Hll) * bl
   bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);

   if (globalStats)
   {
      globalStats->timeLinearSolver = get_time() - t;
      globalStats->hessianPoseDimension = _Hpp->cols();
      globalStats->hessianLandmarkDimension = _Hll->cols();
      globalStats->hessianDimension =
         globalStats->hessianPoseDimension
       + globalStats->hessianLandmarkDimension;
   }
   //cerr << "Solve [decompose and solve] = " <<  get_time()-t << endl;

   if (! solvedPoses)   return false;



   // Resolucion de los landmarks

   // _x contains the solution for the poses, now applying it to the landmark
   // s to get the new part of the solution;
   double* xp = _x;
   double* cp = _coefficients;

   double* xl = _x + _sizePoses;
   double* cl = _coefficients + _sizePoses;
   double* bl = _b + _sizePoses;

   // cp = -xp
   for (int i=0; i<_sizePoses; i++)   cp[i] = -xp[i];

   // cl = bl
   memcpy(cl, bl,_sizeLandmarks * sizeof(double));

   // cl = bl - Bt * xp
   //Bt->multiply(cl, cp);
   _Hpl->rightMultiply(cl, cp);

   // xl = Dinv * cl
   memset(xl, 0, _sizeLandmarks * sizeof(double));
   //_DInvSchur->multiply(xl,cl);
   _DInvSchur->rightMultiply(xl,cl);
   //cerr << "Solve [landmark delta] = " <<  get_time()-t << endl;

   return true;
}

/******************************************************************************/

template <typename Traits>
bool BlockSolver<Traits>::computeMarginals()
{
   double t = get_time();
   double** blocks =  new double*[_optimizer->indexMapping().size()];

   for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      assert(v->uncertaintyData());
      blocks[i] = v->uncertaintyData();
   }
   bool ok = _linearSolver->solveBlocks(blocks, *_Hpp);
   delete [] blocks;

   if (globalStats)  globalStats->timeMarginals = get_time() - t;
   return ok;
}

/******************************************************************************/

template <typename Traits>
bool BlockSolver<Traits>::computeMarginals
(
   SparseBlockMatrix<MatrixXd>& spinv,
   const std::vector<std::pair<int, int> >& blockIndices
)
{
   double t = get_time();
   bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
   if (globalStats)   globalStats->timeMarginals = get_time() - t;
   return ok;
}

/******************************************************************************/

// Calcula la Hessiana y -b y se hace una copia local de -b que se usara al
// resolver el sistema
template <typename Traits>
bool BlockSolver<Traits>::buildSystem()
{
   // clear b vector
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) \
     if (_optimizer->indexMapping().size() > 1000)
# endif
   for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      assert(v);
      // H * x = -b =>
      // clearQuadraticForm() => b = 0
      v->clearQuadraticForm();
   }

   _Hpp->clear();

   if (_doSchur)
   {
      _Hll->clear();
      _Hpl->clear();
   }

  // resetting the terms for the pairwise constraints
  // built up the current system by storing the Hessian
  // blocks in the edges and vertices
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) \
     if (_optimizer->activeEdges().size() > 100)
# endif
   for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k)
   {
      OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
      // Calculo de la Hessiana H y -b del sistema
      e->constructQuadraticForm();
   }

   // flush the current system in a sparse block matrix
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) \
     if (_optimizer->indexMapping().size() > 1000)
# endif
   for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i)
   {
      OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
      int iBase = v->colInHessian();

      // Hace una copia local de b del vertice en b del sistema
      // Construye el vector b
      if (v->marginalized())   iBase += _sizePoses;
      v->copyB( _b+iBase );
   }

   //cerr << *_Hpp << endl;
   //cerr << "\n***************************\n";
   //cerr << *_Hll << endl;
   //cerr << *_Hpl << endl;

   return 0;
}

/******************************************************************************/

// Suma lambda a la diagonal principal (Levenberg–Marquardt)
template <typename Traits>
bool BlockSolver<Traits>::setLambda(double lambda)
{
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numPoses > 100)
# endif
   for (int i = 0; i < _numPoses; i++)
   {
      PoseMatrixType *b = _Hpp->block(i,i);
      b->diagonal().array() += lambda;
   }
# ifdef G2O_OPENMP
# pragma omp parallel for default (shared) if (_numLandmarks > 100)
# endif
   for (int i = 0; i < _numLandmarks; i++)
   {
      LandmarkMatrixType *b=_Hll->block(i,i);
      b->diagonal().array() += lambda;
   }
   return true;
}

/******************************************************************************/

template <typename Traits>
bool BlockSolver<Traits>::init(bool online)
{
   if (! online)
   {
      if (_Hpp)   _Hpp->clear();
      if (_Hpl)   _Hpl->clear();
      if (_Hll)   _Hll->clear();
   }
   _linearSolver->init();
   return true;
}

/******************************************************************************/

} // end namespace