g2o
constant_velocity_target.cpp
Go to the documentation of this file.
1 // This example consists of a single constant velocity target which
2 // moves under piecewise constant velocity in 3D. Its position is
3 // measured by an idealised GPS receiver.
4 
5 #include <Eigen/StdVector>
6 #include <iostream>
7 
8 #include <stdint.h>
10 #include <g2o/core/block_solver.h>
11 #include <g2o/core/solver.h>
15 #include <g2o/stuff/sampler.h>
16 
17 #include "targetTypes6D.hpp"
18 #include "continuous_to_discrete.h"
19 
20 using namespace Eigen;
21 using namespace std;
22 using namespace g2o;
23 
24 int main()
25 {
26  // Set up the parameters of the simulation
27  int numberOfTimeSteps = 1000;
28  const double processNoiseSigma = 1;
29  const double accelerometerNoiseSigma = 1;
30  const double gpsNoiseSigma = 1;
31  const double dt = 1;
32 
33  // Set up the optimiser and block solver
34  SparseOptimizer optimizer;
35  optimizer.setVerbose(false);
36 
38  BlockSolver::LinearSolverType * linearSolver
40  BlockSolver* blockSolver = new BlockSolver(linearSolver);
41  OptimizationAlgorithm* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
42  optimizer.setAlgorithm(optimizationAlgorithm);
43 
44  // Sample the start location of the target
45  Vector6d state;
46  state.setZero();
47  for (int k = 0; k < 3; k++)
48  {
49  state[k] = 1000 * sampleGaussian();
50  }
51 
52  // Construct the first vertex; this corresponds to the initial
53  // condition and register it with the optimiser
55  stateNode->setEstimate(state);
56  stateNode->setId(0);
57  optimizer.addVertex(stateNode);
58 
59  // Set up last estimate
60  VertexPositionVelocity3D* lastStateNode = stateNode;
61 
62  // Iterate over the simulation steps
63  for (int k = 1; k <= numberOfTimeSteps; ++k)
64  {
65  // Simulate the next step; update the state and compute the observation
66  Vector3d processNoise(processNoiseSigma*sampleGaussian(),
67  processNoiseSigma*sampleGaussian(),
68  processNoiseSigma*sampleGaussian());
69 
70  for (int m = 0; m < 3; m++)
71  {
72  state[m] += dt * (state[m+3] + 0.5 * dt * processNoise[m]);
73  }
74 
75  for (int m = 0; m < 3; m++)
76  {
77  state[m+3] += dt * processNoise[m];
78  }
79 
80  // Construct the accelerometer measurement
81  Vector3d accelerometerMeasurement;
82  for (int m = 0; m < 3; m++)
83  {
84  accelerometerMeasurement[m] = processNoise[m] + accelerometerNoiseSigma * sampleGaussian();
85  }
86 
87  // Construct the GPS observation
88  Vector3d gpsMeasurement;
89  for (int m = 0; m < 3; m++)
90  {
91  gpsMeasurement[m] = state[m] + gpsNoiseSigma * sampleGaussian();
92  }
93 
94  // Construct vertex which corresponds to the current state of the target
96 
97  stateNode->setId(k);
98  stateNode->setMarginalized(false);
99  optimizer.addVertex(stateNode);
100 
101  TargetOdometry3DEdge* toe = new TargetOdometry3DEdge(dt, accelerometerNoiseSigma);
102  toe->setVertex(0, lastStateNode);
103  toe->setVertex(1, stateNode);
104  VertexPositionVelocity3D* vPrev= dynamic_cast<VertexPositionVelocity3D*>(lastStateNode);
105  VertexPositionVelocity3D* vCurr= dynamic_cast<VertexPositionVelocity3D*>(stateNode);
106  toe->setMeasurement(accelerometerMeasurement);
107  optimizer.addEdge(toe);
108 
109  // compute the initial guess via the odometry
111  vPrevSet.insert(vPrev);
112  toe->initialEstimate(vPrevSet,vCurr);
113 
114  lastStateNode = stateNode;
115 
116  // Add the GPS observation
117  GPSObservationEdgePositionVelocity3D* goe = new GPSObservationEdgePositionVelocity3D(gpsMeasurement, gpsNoiseSigma);
118  goe->setVertex(0, stateNode);
119  optimizer.addEdge(goe);
120  }
121 
122  // Configure and set things going
123  optimizer.initializeOptimization();
124  optimizer.setVerbose(true);
125  optimizer.optimize(5);
126  cerr << "number of vertices:" << optimizer.vertices().size() << endl;
127  cerr << "number of edges:" << optimizer.edges().size() << endl;
128 
129  // Print the results
130 
131  cout << "state=\n" << state << endl;
132 
133 #if 0
134  for (int k = 0; k < numberOfTimeSteps; k++)
135  {
136  cout << "computed estimate " << k << "\n"
137  << dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find(k)->second)->estimate() << endl;
138  }
139 #endif
140 
141  Vector6d v1 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-2,0))->second)->estimate();
142  Vector6d v2 = dynamic_cast<VertexPositionVelocity3D*>(optimizer.vertices().find((std::max)(numberOfTimeSteps-1,0))->second)->estimate();
143  cout << "v1=\n" << v1 << endl;
144  cout << "v2=\n" << v2 << endl;
145  cout << "delta state=\n" << v2-v1 << endl;
146 }
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:136
int optimize(int iterations, bool online=false)
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.
Traits::LinearSolverType LinearSolverType
Definition: block_solver.h:110
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)
double sampleGaussian(std::mt19937 *generator)
Definition: sampler.cpp:41
const EdgeSet & edges() const
Definition: hyper_graph.h:230
void setAlgorithm(OptimizationAlgorithm *algorithm)
basic solver for Ax = b which has to reimplemented for different linear algebra libraries ...
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
Definition: base_vertex.h:101
virtual void initialEstimate(const g2o::OptimizableGraph::VertexSet &from, g2o::OptimizableGraph::Vertex *to)
void setMarginalized(bool marginalized)
true => this node should be marginalized out during the optimization
Implementation of a solver operating on the blocks of the Hessian.
Definition: block_solver.h:96
Eigen::Matrix< double, 6, 1 > Vector6d
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
Generic interface for a non-linear solver operating on a graph.
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)