g2o
optimization_algorithm_with_hessian.cpp
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 
28 
29 #include "solver.h"
30 #include "optimizable_graph.h"
31 #include "sparse_optimizer.h"
32 
33 #include <iostream>
34 using namespace std;
35 
36 namespace g2o {
37 
38  OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) :
40  _solver(solver)
41  {
42  _writeDebug = _properties.makeProperty<Property<bool> >("writeDebug", true);
43  }
44 
46  {
47  delete _solver;
48  }
49 
51  {
52  assert(_optimizer && "_optimizer not set");
53  assert(_solver && "Solver not set");
55  bool useSchur=false;
56  for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) {
58  if (v->marginalized()){
59  useSchur=true;
60  break;
61  }
62  }
63  if (useSchur){
64  if (_solver->supportsSchur())
65  _solver->setSchur(true);
66  } else {
67  if (_solver->supportsSchur())
68  _solver->setSchur(false);
69  }
70 
71  bool initState = _solver->init(_optimizer, online);
72  return initState;
73  }
74 
75  bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix<MatrixXD>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
76  {
77  return _solver ? _solver->computeMarginals(spinv, blockIndices) : false;
78  }
79 
81  {
82  return _solver ? _solver->buildStructure() : false;
83  }
84 
86  {
87  if (_solver)
89  }
90 
91  bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
92  {
93  return _solver ? _solver->updateStructure(vset, edges) : false;
94  }
95 
97  {
98  _writeDebug->setValue(writeDebug);
99  }
100 
101 } // end namespace
virtual bool updateStructure(const std::vector< HyperGraph::Vertex * > &vset, const HyperGraph::EdgeSet &edges)=0
virtual bool updateStructure(const std::vector< HyperGraph::Vertex * > &vset, const HyperGraph::EdgeSet &edges)
virtual bool computeMarginals(SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices)=0
PropertyMap _properties
the properties of your solver, use this to store the parameters of your solver
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:135
P * makeProperty(const std::string &name_, const typename P::ValueType &v)
Definition: property.h:119
void setValue(const T &v)
Definition: property.h:56
virtual bool supportsSchur()
Definition: solver.h:117
Generic interface for a sparse solver operating on a graph which solves one iteration of the lineariz...
Definition: solver.h:44
virtual bool computeMarginals(SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices)
SparseOptimizer * _optimizer
the optimizer the solver is working on
bool marginalized() const
true => this node is marginalized out during the optimization
A general case Vertex for optimization.
const VertexContainer & activeVertices() const
the vertices active in the current optimization
virtual bool buildSystem()=0
const T & value() const
Definition: property.h:57
virtual void setWriteDebug(bool)=0
Generic interface for a non-linear solver operating on a graph.
virtual bool init(SparseOptimizer *optimizer, bool online=false)=0
Sparse matrix which uses blocks.
virtual void setSchur(bool s)=0
virtual bool buildStructure(bool zeroBlocks=false)=0