static_target.cpp File Reference

#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/solvers/eigen/linear_solver_eigen.h>
#include <g2o/stuff/sampler.h>
#include <iostream>
#include "targetTypes3D.hpp"

Include dependency graph for static_target.cpp:

Go to the source code of this file.

Functions
int	main ()

Function Documentation

main()

int main

(

)

Definition at line 46 of file static_target.cpp.

           {
  // Set up the optimiser
  SparseOptimizer optimizer;
  optimizer.setVerbose(false);
 
  // Create the block solver - the dimensions are specified because
  // 3D observations marginalise to a 3D estimate
  typedef BlockSolver<BlockSolverTraits<3, 3>> BlockSolver_3_3;
  OptimizationAlgorithmGaussNewton* solver =
      new OptimizationAlgorithmGaussNewton(std::make_unique<BlockSolver_3_3>(
          std::make_unique<
              LinearSolverEigen<BlockSolver_3_3::PoseMatrixType>>()));
 
  optimizer.setAlgorithm(solver);
 
  // Sample the actual location of the target
  Vector3d truePoint(sampleUniform(-500, 500), sampleUniform(-500, 500),
                     sampleUniform(-500, 500));
 
  // Construct vertex which corresponds to the actual point of the target
  VertexPosition3D* position = new VertexPosition3D();
  position->setId(0);
  optimizer.addVertex(position);
 
  // Now generate some noise corrupted measurements; for simplicity
  // these are uniformly distributed about the true target. These are
  // modelled as a unary edge because they do not like to, say,
  // another node in the map.
  int numMeasurements = 10;
  double noiseLimit = sqrt(12.);
  double noiseSigma = noiseLimit * noiseLimit / 12.0;
 
  for (int i = 0; i < numMeasurements; i++) {
    Vector3d measurement =
        truePoint + Vector3d(sampleUniform(-0.5, 0.5) * noiseLimit,
                             sampleUniform(-0.5, 0.5) * noiseLimit,
                             sampleUniform(-0.5, 0.5) * noiseLimit);
    GPSObservationPosition3DEdge* goe = new GPSObservationPosition3DEdge();
    goe->setVertex(0, position);
    goe->setMeasurement(measurement);
    goe->setInformation(Matrix3d::Identity() / noiseSigma);
    optimizer.addEdge(goe);
  }
 
  // Configure and set things going
  optimizer.initializeOptimization();
  optimizer.setVerbose(true);
  optimizer.optimize(5);
 
  cout << "truePoint=\n" << truePoint << endl;
  cerr << "computed estimate=\n"
       << dynamic_cast<VertexPosition3D*>(optimizer.vertices().find(0)->second)
              ->estimate()
       << endl;
 
  // position->setMarginalized(true);
 
  SparseBlockMatrix<MatrixXd> spinv;
  bool state = optimizer.computeMarginals(spinv, position);
  if (state) {
    cout << "covariance\n" << spinv << endl;
    cout << spinv.block(0, 0) << endl;
  }
 
  return 0;
}

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().