g2o
optimization_algorithm_gauss_newton.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 <iostream>
30 
31 #include "g2o/stuff/timeutil.h"
32 #include "g2o/stuff/macros.h"
33 
34 #include "solver.h"
35 #include "batch_stats.h"
36 #include "sparse_optimizer.h"
37 using namespace std;
38 
39 namespace g2o {
40 
41  OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) :
43  {
44  }
45 
47  {
48  }
49 
50  OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online)
51  {
52  assert(_optimizer && "_optimizer not set");
53  assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
54  bool ok = true;
55 
56  //here so that correct component for max-mixtures can be computed before the build structure
57  double t=get_monotonic_time();
60  if (globalStats) {
61  globalStats->timeResiduals = get_monotonic_time()-t;
62  }
63 
64  if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
65  ok = _solver->buildStructure();
66  if (! ok) {
67  cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
68  return OptimizationAlgorithm::Fail;
69  }
70  }
71 
74  if (globalStats) {
75  globalStats->timeQuadraticForm = get_monotonic_time()-t;
77  }
78 
79  ok = _solver->solve();
80  if (globalStats) {
81  globalStats->timeLinearSolution = get_monotonic_time()-t;
83  }
84 
85  _optimizer->update(_solver->x());
86  if (globalStats) {
87  globalStats->timeUpdate = get_monotonic_time()-t;
88  }
89  if (ok)
90  return OK;
91  else
92  return Fail;
93  }
94 
96  {
97  os
98  << "\t schur= " << _solver->schur();
99  }
100 
101 } // end namespace
double get_monotonic_time()
Definition: timeutil.cpp:113
virtual bool schur()=0
should the solver perform the schur complement or not
statistics about the optimization
Definition: batch_stats.h:40
#define __PRETTY_FUNCTION__
Definition: macros.h:89
double timeResiduals
residuals
Definition: batch_stats.h:49
Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg.
utility functions for handling time related stuff
Generic interface for a sparse solver operating on a graph which solves one iteration of the lineariz...
Definition: solver.h:44
SparseOptimizer * _optimizer
the optimizer the solver is working on
static G2OBatchStatistics * globalStats()
Definition: batch_stats.h:73
SparseOptimizer * optimizer() const
the optimizer (graph) on which the solver works
Definition: solver.h:106
virtual bool solve()=0
virtual void printVerbose(std::ostream &os) const
double * x()
return x, the solution vector
Definition: solver.h:96
double timeUpdate
time to apply the update
Definition: batch_stats.h:62
virtual SolverResult solve(int iteration, bool online=false)
virtual bool buildSystem()=0
double timeQuadraticForm
construct the quadratic form in the graph
Definition: batch_stats.h:51
double timeLinearSolution
total time for solving Ax=b (including detup for schur)
Definition: batch_stats.h:59
virtual bool buildStructure(bool zeroBlocks=false)=0