g2o_hierarchical_test_functions.cpp File Reference

#include <signal.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <algorithm>
#include <cassert>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include "edge_creator.h"
#include "edge_labeler.h"
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/g2o_common.h"
#include "g2o/apps/g2o_cli/output_helper.h"
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/factory.h"
#include "g2o/core/hyper_dijkstra.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.h"
#include "g2o/stuff/unscented.h"
#include "star.h"

Include dependency graph for g2o_hierarchical_test_functions.cpp:

Go to the source code of this file.

Typedefs
typedef SigmaPoint< VectorXd >	MySigmaPoint

Functions
void	testMarginals (SparseOptimizer &optimizer)
int	unscentedTest ()

Typedef Documentation

MySigmaPoint

typedef SigmaPoint<VectorXd> MySigmaPoint

Definition at line 62 of file g2o_hierarchical_test_functions.cpp.

Function Documentation

testMarginals()

void testMarginals

(

SparseOptimizer &

optimizer

)

Definition at line 64 of file g2o_hierarchical_test_functions.cpp.

                                               {
  cerr << "Projecting marginals" << endl;
  std::vector<std::pair<int, int> > blockIndices;
  for (size_t i = 0; i < optimizer.activeVertices().size(); i++) {
    OptimizableGraph::Vertex* v = optimizer.activeVertices()[i];
    if (v->hessianIndex() >= 0) {
      blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
    }
    // if (v->hessianIndex()>0){
    //   blockIndices.push_back(make_pair(v->hessianIndex()-1,
    //   v->hessianIndex()));
    // }
  }
  SparseBlockMatrix<MatrixXd> spinv;
  if (optimizer.computeMarginals(spinv, blockIndices)) {
    for (size_t i = 0; i < optimizer.activeVertices().size(); i++) {
      OptimizableGraph::Vertex* v = optimizer.activeVertices()[i];
      cerr << "Vertex id:" << v->id() << endl;
      if (v->hessianIndex() >= 0) {
        cerr << "increments block :" << v->hessianIndex() << ", "
             << v->hessianIndex() << " covariance:" << endl;
        VectorXd mean(
            v->minimalEstimateDimension());  // HACK: need to set identity
        mean.fill(0);
        VectorXd oldMean(
            v->minimalEstimateDimension());  // HACK: need to set identity
        v->getMinimalEstimateData(&oldMean[0]);
        MatrixXd& cov = *(spinv.block(v->hessianIndex(), v->hessianIndex()));
        std::vector<MySigmaPoint> spts;
        cerr << cov << endl;
        if (!sampleUnscented(spts, mean, cov)) continue;
 
        // now apply the oplus operator to the sigma points,
        // and get the points in the global space
        std::vector<MySigmaPoint> tspts = spts;
 
        for (size_t j = 0; j < spts.size(); j++) {
          v->push();
          // cerr << "v_before [" << j << "]" << endl;
          v->getMinimalEstimateData(&mean[0]);
          // cerr << mean << endl;
          // cerr << "sigma [" << j << "]" << endl;
          // cerr << spts[j]._sample << endl;
          v->oplus(&(spts[j]._sample[0]));
          v->getMinimalEstimateData(&mean[0]);
          tspts[j]._sample = mean;
          // cerr << "oplus [" << j << "]" << endl;
          // cerr << tspts[j]._sample << endl;
          v->pop();
        }
        MatrixXd cov2 = cov;
        reconstructGaussian(mean, cov2, tspts);
        cerr << "global block :" << v->hessianIndex() << ", "
             << v->hessianIndex() << endl;
        cerr << "mean: " << endl;
        cerr << mean << endl;
        cerr << "oldMean: " << endl;
        cerr << oldMean << endl;
        cerr << "cov: " << endl;
        cerr << cov2 << endl;
      }
      // if (v->hessianIndex()>0){
      //   cerr << "inv block :" << v->hessianIndex()-1 << ", " <<
      //   v->hessianIndex()<< endl; cerr << *(spinv.block(v->hessianIndex()-1,
      //   v->hessianIndex())); cerr << endl;
      // }
    }
  }
}

References g2o::SparseOptimizer::activeVertices(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::SparseOptimizer::computeMarginals(), g2o::OptimizableGraph::Vertex::getMinimalEstimateData(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::HyperGraph::Vertex::id(), g2o::OptimizableGraph::Vertex::minimalEstimateDimension(), g2o::OptimizableGraph::Vertex::oplus(), g2o::OptimizableGraph::Vertex::pop(), g2o::OptimizableGraph::Vertex::push(), g2o::reconstructGaussian(), and g2o::sampleUnscented().

unscentedTest()

int unscentedTest

(

)

Definition at line 134 of file g2o_hierarchical_test_functions.cpp.

                    {
  MatrixXd m = MatrixXd(6, 6);
  for (int i = 0; i < 6; i++) {
    for (int j = i; j < 6; j++) {
      m(i, j) = m(j, i) = i * j + 1;
    }
  }
  m += MatrixXd::Identity(6, 6);
  cerr << m;
  VectorXd mean(6);
  mean.fill(1);
 
  std::vector<MySigmaPoint> spts;
  sampleUnscented(spts, mean, m);
  for (size_t i = 0; i < spts.size(); i++) {
    cerr << "Point " << i << " " << endl
         << "wi=" << spts[i]._wi << " wp=" << spts[i]._wp << " " << endl;
    cerr << spts[i]._sample << endl;
  }
 
  VectorXd recMean(6);
  MatrixXd recCov(6, 6);
 
  reconstructGaussian(recMean, recCov, spts);
 
  cerr << "recMean" << endl;
  cerr << recMean << endl;
 
  cerr << "recCov" << endl;
  cerr << recCov << endl;
 
  return 0;
}

References g2o::reconstructGaussian(), and g2o::sampleUnscented().