polynomial_fit.cpp File Reference

#include <unsupported/Eigen/Polynomials>
#include "g2o/core/base_binary_edge.h"
#include "g2o/core/base_dynamic_vertex.h"
#include "g2o/core/base_unary_edge.h"
#include "g2o/core/block_solver.h"
#include "g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/solvers/eigen/linear_solver_eigen.h"
#include "g2o/stuff/sampler.h"

Include dependency graph for polynomial_fit.cpp:

Go to the source code of this file.

Classes
class	PolynomialCoefficientVertex
class	PolynomialSingleValueEdge

Functions
int	main (int argc, const char *argv[])

Function Documentation

main()

int main	(	int	argc,
		const char *	argv[]
	)

Definition at line 133 of file polynomial_fit.cpp.

                                       {
  // Number of dimensions of the polynomial; the default is 4
  int polynomialDimension = 4;
  if (argc > 1) {
    polynomialDimension = atoi(argv[1]);
  }
 
  // Create the coefficients for the polynomial (all drawn randomly)
  Eigen::VectorXd p(polynomialDimension);
  for (int i = 0; i < polynomialDimension; ++i) {
    p[i] = g2o::sampleUniform(-1, 1);
  }
 
  std::cout << "Ground truth vector=" << p.transpose() << std::endl;
 
  // The number of observations in the polynomial; the default is 6
  int obs = 6;
  if (argc > 2) {
    obs = atoi(argv[2]);
  }
 
  // Sample the observations; we don't do anything with them here, but
  // they could be plotted
  double sigmaZ = 0.1;
  Eigen::VectorXd x(obs);
  Eigen::VectorXd z(obs);
 
  for (int i = 0; i < obs; ++i) {
    x[i] = g2o::sampleUniform(-5, 5);
    z[i] = Eigen::poly_eval(p, x[i]) + sigmaZ * g2o::sampleGaussian();
  }
 
  // Construct the graph and set up the solver and optimiser
  std::unique_ptr<g2o::BlockSolverX::LinearSolverType> linearSolver =
      std::make_unique<
          g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>>();
 
  // Set up the solver
  std::unique_ptr<g2o::BlockSolverX> blockSolver =
      std::make_unique<g2o::BlockSolverX>(std::move(linearSolver));
 
  // Set up the optimisation algorithm
  g2o::OptimizationAlgorithm* optimisationAlgorithm =
      new g2o::OptimizationAlgorithmLevenberg(std::move(blockSolver));
 
  // Create the graph and configure it
  std::unique_ptr<g2o::SparseOptimizer> optimizer =
      std::make_unique<g2o::SparseOptimizer>();
  optimizer->setVerbose(true);
  optimizer->setAlgorithm(optimisationAlgorithm);
 
  // Create the vertex; note its dimension is currently is undefined
  PolynomialCoefficientVertex* pv = new PolynomialCoefficientVertex();
  pv->setId(0);
  optimizer->addVertex(pv);
 
  // Create the information matrix
  PolynomialSingleValueEdge::InformationType omega =
      PolynomialSingleValueEdge::InformationType::Zero();
  omega(0, 0) = 1 / (sigmaZ * sigmaZ);
 
  // Create the observations and the edges
  for (int i = 0; i < obs; ++i) {
    PolynomialSingleValueEdge* pe =
        new PolynomialSingleValueEdge(x[i], z[i], omega);
    pe->setVertex(0, pv);
    optimizer->addEdge(pe);
  }
 
  // Now run the same optimization problem for different choices of
  // dimension of the polynomial vertex. This shows how we can
  // dynamically change the vertex dimensions in an alreacy
  // constructed graph. Note that you must call initializeOptimization
  // before you can optimize after a state dimension has changed.
  for (int testDimension = 1; testDimension <= polynomialDimension;
       ++testDimension) {
    pv->setDimension(testDimension);
    optimizer->initializeOptimization();
    optimizer->optimize(10);
    std::cout << "Computed parameters = " << pv->estimate().transpose()
              << std::endl;
  }
  for (int testDimension = polynomialDimension - 1; testDimension >= 1;
       --testDimension) {
    pv->setDimension(testDimension);
    optimizer->initializeOptimization();
    optimizer->optimize(10);
    std::cout << "Computed parameters = " << pv->estimate().transpose()
              << std::endl;
  }
}

References g2o::BaseVertex< D, T >::estimate(), g2o::sampleGaussian(), g2o::sampleUniform(), g2o::BaseDynamicVertex< T >::setDimension(), g2o::OptimizableGraph::Vertex::setId(), and g2o::HyperGraph::Edge::setVertex().