g2o
Functions
static_target.cpp File Reference
#include <Eigen/StdVector>
#include <iostream>
#include <stdint.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/stuff/sampler.h>
#include "targetTypes3D.hpp"
Include dependency graph for static_target.cpp:

Go to the source code of this file.

Functions

int main ()
 

Function Documentation

int main ( )

Definition at line 22 of file static_target.cpp.

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseOptimizer::computeMarginals(), g2o::SparseOptimizer::initializeOptimization(), g2o::SparseOptimizer::optimize(), g2o::sampleUniform(), g2o::SparseOptimizer::setAlgorithm(), g2o::OptimizableGraph::Vertex::setId(), g2o::BaseEdge< D, E >::setInformation(), g2o::BaseEdge< D, E >::setMeasurement(), g2o::SparseOptimizer::setVerbose(), g2o::HyperGraph::Edge::setVertex(), and g2o::HyperGraph::vertices().

23 {
24  // Set up the optimiser
25  SparseOptimizer optimizer;
26  optimizer.setVerbose(false);
27 
28  // Create the block solver - the dimensions are specified because
29  // 3D observations marginalise to a 3D estimate
30  typedef BlockSolver<BlockSolverTraits<3, 3> > BlockSolver_3_3;
31  BlockSolver_3_3::LinearSolverType* linearSolver
33  BlockSolver_3_3* blockSolver
34  = new BlockSolver_3_3(linearSolver);
36  = new OptimizationAlgorithmGaussNewton(blockSolver);
37  optimizer.setAlgorithm(solver);
38 
39  // Sample the actual location of the target
40  Vector3d truePoint(sampleUniform(-500, 500),
41  sampleUniform(-500, 500),
42  sampleUniform(-500, 500));
43 
44  // Construct vertex which corresponds to the actual point of the target
45  VertexPosition3D* position = new VertexPosition3D();
46  position->setId(0);
47  optimizer.addVertex(position);
48 
49  // Now generate some noise corrupted measurements; for simplicity
50  // these are uniformly distributed about the true target. These are
51  // modelled as a unary edge because they do not like to, say,
52  // another node in the map.
53  int numMeasurements = 10;
54  double noiseLimit = sqrt(12.);
55  double noiseSigma = noiseLimit*noiseLimit / 12.0;
56 
57  for (int i = 0; i < numMeasurements; i++)
58  {
59  Vector3d measurement = truePoint +
60  Vector3d(sampleUniform(-0.5, 0.5) * noiseLimit,
61  sampleUniform(-0.5, 0.5) * noiseLimit,
62  sampleUniform(-0.5, 0.5) * noiseLimit);
64  goe->setVertex(0, position);
65  goe->setMeasurement(measurement);
66  goe->setInformation(Matrix3d::Identity() / noiseSigma);
67  optimizer.addEdge(goe);
68  }
69 
70  // Configure and set things going
71  optimizer.initializeOptimization();
72  optimizer.setVerbose(true);
73  optimizer.optimize(5);
74 
75  cout << "truePoint=\n" << truePoint << endl;
76 
77  cerr << "computed estimate=\n" << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)->estimate() << endl;
78 
79  //position->setMarginalized(true);
80 
82 
83  optimizer.computeMarginals(spinv, position);
84 
85 
86 
87  //optimizer.solver()->computeMarginals();
88 
89  // covariance
90  //
91  cout << "covariance\n" << spinv << endl;
92 
93  cout << spinv.block(0,0) << endl;
94 
95 }
int optimize(int iterations, bool online=false)
SparseMatrixBlock * block(int r, int c, bool alloc=false)
returns the block at location r,c. if alloc=true he block is created if it does not exist ...
virtual void setMeasurement(const Measurement &m)
Definition: base_edge.h:76
void setVertex(size_t i, Vertex *v)
Definition: hyper_graph.h:194
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
Implementation of the Gauss Newton Algorithm.
virtual void setId(int id)
sets the id of the node in the graph be sure that the graph keeps consistent after changing the id ...
void setVerbose(bool verbose)
void setAlgorithm(OptimizationAlgorithm *algorithm)
EIGEN_STRONG_INLINE void setInformation(const InformationType &information)
Definition: base_edge.h:69
basic solver for Ax = b which has to reimplemented for different linear algebra libraries ...
Implementation of a solver operating on the blocks of the Hessian.
Definition: block_solver.h:96
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
bool computeMarginals(SparseBlockMatrix< MatrixXD > &spinv, const std::vector< std::pair< int, int > > &blockIndices)
Sparse matrix which uses blocks.
double sampleUniform(double min, double max, std::mt19937 *generator)
Definition: sampler.cpp:35