g2o
jacobian_workspace.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 
27 #include "jacobian_workspace.h"
28 
29 #include <cmath>
30 
31 #include "optimizable_graph.h"
32 
33 using namespace std;
34 
35 namespace g2o {
36 
37 JacobianWorkspace::JacobianWorkspace() :
38  _maxNumVertices(-1), _maxDimension(-1)
39 {
40 }
41 
43 {
44 }
45 
47 {
48  //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
49  if (_maxNumVertices <=0 || _maxDimension <= 0)
50  return false;
52  for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) {
53  it->resize(_maxDimension);
54  it->setZero();
55  }
56  return true;
57 }
58 
60 {
61  const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(e_);
62  int errorDimension = e->dimension();
63  int numVertices = e->vertices().size();
64  int maxDimensionForEdge = -1;
65  for (int i = 0; i < numVertices; ++i) {
66  const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
67  assert(v && "Edge has no vertex assigned");
68  maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge);
69  }
70  _maxNumVertices = max(numVertices, _maxNumVertices);
71  _maxDimension = max(maxDimensionForEdge, _maxDimension);
72  //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
73 }
74 
76 {
77  for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) {
78  const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
79  updateSize(e);
80  }
81 }
82 
83 void JacobianWorkspace::updateSize(int numVertices, int dimension)
84 {
85  _maxNumVertices = max(numVertices, _maxNumVertices);
86  _maxDimension = max(dimension, _maxDimension);
87 }
88 
89 } // end namespace
int _maxDimension
the maximum dimension (number of elements) for a Jacobian
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:186
int dimension() const
returns the dimensions of the error function
int dimension() const
dimension of the estimated state belonging to this node
const VertexContainer & vertices() const
Definition: hyper_graph.h:178
const EdgeSet & edges() const
Definition: hyper_graph.h:230
void updateSize(const HyperGraph::Edge *e)
WorkspaceVector _workspace
the memory pre-allocated for computing the Jacobians
A general case Vertex for optimization.
int _maxNumVertices
the maximum number of vertices connected by a hyper-edge
Protocol The SLAM executable accepts such as solving the and retrieving or vertices in the graph
Definition: protocol.txt:7