block_solver.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #ifndef G2O_BLOCK_SOLVER_H
28 #define G2O_BLOCK_SOLVER_H
29 #include <Eigen/Core>
30 #include "solver.h"
31 #include "linear_solver.h"
32 #include "sparse_block_matrix.h"
34 #include "openmp_mutex.h"
35 #include "../../config.h"
36 
37 namespace g2o {
38  using namespace Eigen;
39 
43  template <int _PoseDim, int _LandmarkDim>
45  {
46  static const int PoseDim = _PoseDim;
47  static const int LandmarkDim = _LandmarkDim;
48  typedef Matrix<double, PoseDim, PoseDim> PoseMatrixType;
49  typedef Matrix<double, LandmarkDim, LandmarkDim> LandmarkMatrixType;
50  typedef Matrix<double, PoseDim, LandmarkDim> PoseLandmarkMatrixType;
51  typedef Matrix<double, PoseDim, 1> PoseVectorType;
52  typedef Matrix<double, LandmarkDim, 1> LandmarkVectorType;
53 
58  };
59 
63  template <>
64  struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
65  {
66  static const int PoseDim = Eigen::Dynamic;
67  static const int LandmarkDim = Eigen::Dynamic;
68  typedef MatrixXd PoseMatrixType;
69  typedef MatrixXd LandmarkMatrixType;
70  typedef MatrixXd PoseLandmarkMatrixType;
71  typedef VectorXd PoseVectorType;
72  typedef VectorXd LandmarkVectorType;
73 
78  };
79 
83  class BlockSolverBase : public Solver
84  {
85  public:
86  virtual ~BlockSolverBase() {}
90  virtual void multiplyHessian(double* dest, const double* src) const = 0;
91  };
92 
96  template <typename Traits>
97  class BlockSolver: public BlockSolverBase {
98  public:
99 
100  static const int PoseDim = Traits::PoseDim;
101  static const int LandmarkDim = Traits::LandmarkDim;
102  typedef typename Traits::PoseMatrixType PoseMatrixType;
103  typedef typename Traits::LandmarkMatrixType LandmarkMatrixType;
104  typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
105  typedef typename Traits::PoseVectorType PoseVectorType;
106  typedef typename Traits::LandmarkVectorType LandmarkVectorType;
107 
108  typedef typename Traits::PoseHessianType PoseHessianType;
109  typedef typename Traits::LandmarkHessianType LandmarkHessianType;
110  typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
111  typedef typename Traits::LinearSolverType LinearSolverType;
112 
113  public:
114 
120  BlockSolver(LinearSolverType* linearSolver);
121  ~BlockSolver();
122 
123  virtual bool init(SparseOptimizer* optmizer, bool online = false);
124  virtual bool buildStructure(bool zeroBlocks = false);
125  virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
126  virtual bool buildSystem();
127  virtual bool solve();
128  virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
129  virtual bool setLambda(double lambda, bool backup = false);
130  virtual void restoreDiagonal();
131  virtual bool supportsSchur() {return true;}
132  virtual bool schur() { return _doSchur;}
133  virtual void setSchur(bool s) { _doSchur = s;}
134 
135  LinearSolver<PoseMatrixType>* linearSolver() const { return _linearSolver;}
136 
137  virtual void setWriteDebug(bool writeDebug);
138  virtual bool writeDebug() const {return _linearSolver->writeDebug();}
139 
140  virtual bool saveHessian(const std::string& fileName) const;
141 
142  virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
143 
144  protected:
145  void resize(int* blockPoseIndices, int numPoseBlocks,
146  int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
147 
148  void deallocate();
149 
153 
156 
159 
161 
162  std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
163  std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
164 
165 # ifdef G2O_OPENMP
166  std::vector<OpenMPMutex> _coefficientsMutex;
167 # endif
168 
169  bool _doSchur;
170 
171  double* _coefficients;
172  double* _bschur;
173 
174  int _numPoses, _numLandmarks;
175  int _sizePoses, _sizeLandmarks;
176  };
177 
178 
179  //variable size solver
181  // solver for BA/3D SLAM
183  // solver fo BA with scale
185  // 2Dof landmarks 3Dof poses
187 
188 } // end namespace
189 
190 #include "block_solver.hpp"
191 
192 
193 #endif
SparseBlockMatrix< PoseMatrixType > * _Hschur
Definition: block_solver.h:154
Traits::PoseHessianType PoseHessianType
Definition: block_solver.h:108
std::vector< LandmarkVectorType, Eigen::aligned_allocator< LandmarkVectorType > > _diagonalBackupLandmark
Definition: block_solver.h:163
Traits::LandmarkMatrixType LandmarkMatrixType
Definition: block_solver.h:103
BlockSolver< BlockSolverTraits< 6, 3 > > BlockSolver_6_3
Definition: block_solver.h:182
Traits::PoseVectorType PoseVectorType
Definition: block_solver.h:105
SparseBlockMatrix< PoseMatrixType > * _Hpp
Definition: block_solver.h:150
virtual bool schur()
should the solver perform the schur complement or not
Definition: block_solver.h:132
Traits::LandmarkHessianType LandmarkHessianType
Definition: block_solver.h:109
virtual void setSchur(bool s)
Definition: block_solver.h:133
SparseBlockMatrixDiagonal< LandmarkMatrixType > * _DInvSchur
Definition: block_solver.h:155
SparseBlockMatrixCCS< PoseLandmarkMatrixType > * _HplCCS
Definition: block_solver.h:157
traits to summarize the properties of the fixed size optimization problem
Definition: block_solver.h:44
Matrix< double, PoseDim, LandmarkDim > PoseLandmarkMatrixType
Definition: block_solver.h:50
SparseBlockMatrix< LandmarkMatrixType > * _Hll
Definition: block_solver.h:151
Matrix< double, LandmarkDim, LandmarkDim > LandmarkMatrixType
Definition: block_solver.h:49
SparseBlockMatrix< PoseLandmarkMatrixType > PoseLandmarkHessianType
Definition: block_solver.h:56
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:102
double * _coefficients
Definition: block_solver.h:171
Traits::LinearSolverType LinearSolverType
Definition: block_solver.h:111
SparseBlockMatrix< PoseMatrixType > PoseHessianType
Definition: block_solver.h:54
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:90
BlockSolver< BlockSolverTraits< 3, 2 > > BlockSolver_3_2
Definition: block_solver.h:186
SparseBlockMatrixCCS< PoseMatrixType > * _HschurTransposedCCS
Definition: block_solver.h:158
Traits::PoseLandmarkHessianType PoseLandmarkHessianType
Definition: block_solver.h:110
LinearSolver< PoseMatrixType > * linearSolver() const
Definition: block_solver.h:135
std::vector< PoseVectorType, Eigen::aligned_allocator< PoseVectorType > > _diagonalBackupPose
Definition: block_solver.h:162
Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType
Definition: block_solver.h:104
Generic interface for a sparse solver operating on a graph which solves one iteration of the lineariz...
Definition: solver.h:43
Traits::LandmarkVectorType LandmarkVectorType
Definition: block_solver.h:106
SparseBlockMatrix< PoseLandmarkMatrixType > PoseLandmarkHessianType
Definition: block_solver.h:76
virtual bool supportsSchur()
Definition: block_solver.h:131
SparseBlockMatrix< PoseLandmarkMatrixType > * _Hpl
Definition: block_solver.h:152
Matrix< double, LandmarkDim, 1 > LandmarkVectorType
Definition: block_solver.h:52
SparseBlockMatrix< PoseMatrixType > PoseHessianType
Definition: block_solver.h:74
virtual void multiplyHessian(double *dest, const double *src) const
Definition: block_solver.h:142
SparseBlockMatrix< LandmarkMatrixType > LandmarkHessianType
Definition: block_solver.h:55
BlockSolver< BlockSolverTraits< 7, 3 > > BlockSolver_7_3
Definition: block_solver.h:184
LinearSolver< PoseMatrixType > * _linearSolver
Definition: block_solver.h:160
Implementation of a solver operating on the blocks of the Hessian.
Definition: block_solver.h:97
LinearSolver< PoseMatrixType > LinearSolverType
Definition: block_solver.h:57
SparseBlockMatrix< LandmarkMatrixType > LandmarkHessianType
Definition: block_solver.h:75
virtual ~BlockSolverBase()
Definition: block_solver.h:86
virtual bool writeDebug() const
Definition: block_solver.h:138
base for the block solvers with some basic function interfaces
Definition: block_solver.h:83
BlockSolver< BlockSolverTraits< Eigen::Dynamic, Eigen::Dynamic > > BlockSolverX
Definition: block_solver.h:180
Matrix< double, PoseDim, 1 > PoseVectorType
Definition: block_solver.h:51
Matrix< double, PoseDim, PoseDim > PoseMatrixType
Definition: block_solver.h:48


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47