block_solver.h
Go to the documentation of this file.
00001 // g2o - General Graph Optimization
00002 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
00003 // 
00004 // g2o is free software: you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published
00006 // by the Free Software Foundation, either version 3 of the License, or
00007 // (at your option) any later version.
00008 // 
00009 // g2o is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 // 
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016 
00017 #ifndef BLOCK_SOLVER_H
00018 #define BLOCK_SOLVER_H
00019 #include <Eigen/Core>
00020 #include "solver.h"
00021 #include "linear_solver.h"
00022 #include "sparse_block_matrix.h"
00023 #include "g2o/config.h"
00024 
00025 namespace g2o {
00026   using namespace Eigen;
00027 
00032   template <int _PoseDim, int _LandmarkDim>
00033   struct BlockSolverTraits
00034   {
00035     static const int PoseDim = _PoseDim;
00036     static const int LandmarkDim = _LandmarkDim;
00037     typedef Matrix<double, PoseDim, PoseDim> PoseMatrixType;
00038     typedef Matrix<double, LandmarkDim, LandmarkDim> LandmarkMatrixType;
00039     typedef Matrix<double, PoseDim, LandmarkDim> PoseLandmarkMatrixType;
00040     typedef Matrix<double, PoseDim, 1> PoseVectorType;
00041     typedef Matrix<double, LandmarkDim, 1> LandmarkVectorType;
00042 
00043     typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
00044     typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
00045     typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
00046     typedef LinearSolver<PoseMatrixType> LinearSolverType;
00047   };
00048 
00053   template <>
00054   struct BlockSolverTraits<-1, -1>
00055   {
00056     static const int PoseDim = -1;
00057     static const int LandmarkDim = -1;
00058     typedef MatrixXd PoseMatrixType;
00059     typedef MatrixXd LandmarkMatrixType;
00060     typedef MatrixXd PoseLandmarkMatrixType;
00061     typedef VectorXd PoseVectorType;
00062     typedef VectorXd LandmarkVectorType;
00063 
00064 
00065     typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
00066     typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
00067     typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
00068     typedef LinearSolver<PoseMatrixType> LinearSolverType;
00069   };
00070 
00074   template <typename Traits>
00075   class BlockSolver: public Solver {
00076     public:
00077 
00078       static const int PoseDim = Traits::PoseDim;
00079       static const int LandmarkDim = Traits::LandmarkDim;
00080       typedef typename Traits::PoseMatrixType PoseMatrixType;
00081       typedef typename Traits::LandmarkMatrixType LandmarkMatrixType; 
00082       typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
00083       typedef typename Traits::PoseVectorType PoseVectorType;
00084       typedef typename Traits::LandmarkVectorType LandmarkVectorType;
00085 
00086       typedef typename Traits::PoseHessianType PoseHessianType;
00087       typedef typename Traits::LandmarkHessianType LandmarkHessianType;
00088       typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
00089       typedef typename Traits::LinearSolverType LinearSolverType;
00090 
00091     public:
00092 
00098       BlockSolver(SparseOptimizer* optimizer, LinearSolverType* linearSolver);
00099       ~BlockSolver();
00100 
00101       virtual bool init(bool online = false);
00102       virtual bool buildStructure(bool zeroBlocks = false);
00103       virtual bool updateStructure
00104       (
00105          const std::vector<HyperGraph::Vertex*>& vset,
00106          const HyperGraph::EdgeSet& edges
00107       );
00108       virtual bool buildSystem();
00109       virtual bool solve();
00110       virtual bool computeMarginals();
00111       virtual bool computeMarginals
00112       (
00113          SparseBlockMatrix<MatrixXd>& spinv,
00114          const std::vector<std::pair<int, int> >& blockIndices
00115       );
00116       virtual bool setLambda(double lambda);
00117       virtual bool supportsSchur() {return true;}
00118       virtual bool schur() { return _doSchur;}
00119       virtual void setSchur(bool s) { _doSchur = s;}
00120 
00121       LinearSolver<PoseMatrixType>* linearSolver() const {return _linearSolver;}
00122 
00123     protected:
00124       void resize
00125       (
00126          int* blockPoseIndices, int numPoseBlocks, 
00127          int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim
00128       );
00129 
00130       SparseBlockMatrix<PoseMatrixType>* _Hpp;
00131       SparseBlockMatrix<LandmarkMatrixType>* _Hll;
00132       SparseBlockMatrix<PoseLandmarkMatrixType>* _Hpl;
00133 
00134       SparseBlockMatrix<PoseMatrixType>* _Hschur;
00135       SparseBlockMatrix<LandmarkMatrixType>* _DInvSchur;
00136 
00137       LinearSolver<PoseMatrixType>* _linearSolver;
00138 
00139       bool _doSchur;
00140 
00141       double* _coefficients;
00142       double* _bschur;
00143 
00144       int _numPoses, _numLandmarks;
00145       int _sizePoses, _sizeLandmarks;
00146   };
00147 
00148 
00149   //variable size solver
00150   typedef BlockSolver< BlockSolverTraits<-1, -1> > BlockSolverX;
00151   // solver for BA/3D SLAM
00152   typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;  
00153   // solver fo BA with scale
00154   typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;  
00155   // 2Dof landmarks 3Dof poses
00156   typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
00157 
00158 } // end namespace
00159 
00160 #include "block_solver.hpp"
00161 
00162 
00163 #endif


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:30:48