test_slam3d_jacobian.cpp File Reference

#include <iostream>
#include <cstdio>
#include "g2o/core/jacobian_workspace.h"
#include "g2o/stuff/macros.h"
#include "edge_se3.h"
#include "edge_se3_prior.h"

Include dependency graph for test_slam3d_jacobian.cpp:

Go to the source code of this file.

Functions
Isometry3D	randomIsometry3d ()
int	main (int, char **)

Function Documentation

int main	(	int	,
		char **
	)

Definition at line 49 of file test_slam3d_jacobian.cpp.

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addParameter(), g2o::OptimizableGraph::addVertex(), g2o::JacobianWorkspace::allocate(), g2o::BaseEdge< D, E >::chi2(), g2o::EdgeSE3Prior::computeError(), g2o::BaseVertex< D, T >::estimate(), graph, g2o::EdgeSE3Prior::initialEstimate(), g2o::BaseEdge< D, E >::measurement(), randomIsometry3d(), g2o::BaseVertex< D, T >::setEstimate(), g2o::Parameter::setId(), g2o::OptimizableGraph::Vertex::setId(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3::setMeasurement(), g2o::EdgeSE3Prior::setMeasurement(), g2o::OptimizableGraph::Edge::setParameterId(), g2o::HyperGraph::Edge::setVertex(), g2o::JacobianWorkspace::updateSize(), and g2o::JacobianWorkspace::workspaceForVertex().

{
  if (0) {
    OptimizableGraph graph;
    ParameterSE3Offset* offsetParam = new ParameterSE3Offset;
    offsetParam->setId(0);
    graph.addParameter(offsetParam);

    Isometry3D zeroPose = Isometry3D::Identity();
    VertexSE3* v1 = new VertexSE3;
    v1->setId(0);
    graph.addVertex(v1);
    OptimizableGraph::VertexSet auxSet;
    EdgeSE3Prior* priorEdge = new EdgeSE3Prior;
    priorEdge->setVertex(0, v1);
    priorEdge->setMeasurement(randomIsometry3d());
    priorEdge->setParameterId(0, 0);
    cout << PVAR(priorEdge->measurement().matrix()) << endl;
    graph.addEdge(priorEdge);

    Eigen::Matrix<double,6,6,Eigen::ColMajor> information = Eigen::Matrix<double,6,6,Eigen::ColMajor>::Identity();

    cout << "Full information" << endl;
    v1->setEstimate(zeroPose);
    priorEdge->setInformation(information);
    priorEdge->initialEstimate(auxSet, 0);
    cout << PVAR(priorEdge->chi2()) << endl;
    cout << v1->estimate().matrix() << endl << endl;
    priorEdge->computeError();

    cout << "Only translation" << endl;
    v1->setEstimate(zeroPose);
    information.block<3,3>(0,0) = Matrix3D::Identity();
    information.block<3,3>(3,3) = Matrix3D::Zero();
    priorEdge->setInformation(information);
    priorEdge->initialEstimate(auxSet, 0);
    cout << PVAR(priorEdge->chi2()) << endl;
    cout << v1->estimate().matrix() << endl << endl;
    priorEdge->computeError();

    cout << "Only rotation" << endl;
    v1->setEstimate(zeroPose);
    information.block<3,3>(0,0) = Matrix3D::Zero();
    information.block<3,3>(3,3) = Matrix3D::Identity();
    priorEdge->setInformation(information);
    priorEdge->initialEstimate(auxSet, 0);
    cout << PVAR(priorEdge->chi2()) << endl;
    cout << v1->estimate().matrix() << endl << endl;
    priorEdge->computeError();
    return 0;
  }

  VertexSE3 v1;
  v1.setId(0); 

  VertexSE3 v2;
  v2.setId(1); 

  EdgeSE3 e;
  e.setVertex(0, &v1);
  e.setVertex(1, &v2);
  e.setInformation(Eigen::Matrix<double,6,6,Eigen::ColMajor>::Identity());

  JacobianWorkspace jacobianWorkspace;
  JacobianWorkspace numericJacobianWorkspace;
  numericJacobianWorkspace.updateSize(&e);
  numericJacobianWorkspace.allocate();

  for (int k = 0; k < 100000; ++k) {

    v1.setEstimate(randomIsometry3d());
    v2.setEstimate(randomIsometry3d());
    e.setMeasurement(randomIsometry3d());

    // calling the analytic Jacobian but writing to the numeric workspace
    e.BaseBinaryEdge<6, Isometry3D, VertexSE3, VertexSE3>::linearizeOplus(numericJacobianWorkspace);
    // copy result into analytic workspace
    jacobianWorkspace = numericJacobianWorkspace;

    // compute the numeric Jacobian into the numericJacobianWorkspace workspace as setup by the previous call
    e.BaseBinaryEdge<6, Isometry3D, VertexSE3, VertexSE3>::linearizeOplus();

    // compare the two Jacobians
    const double allowedDifference = 1e-6;
    for (int i = 0; i < 2; ++i) {
      double* n = numericJacobianWorkspace.workspaceForVertex(i);
      double* a = jacobianWorkspace.workspaceForVertex(i);
      for (int j = 0; j < 6*6; ++j) {
        double d = fabs(n[j] - a[j]);
        if (d > allowedDifference) {
          cerr << "\ndetected difference in the Jacobians " << d << endl;
          cerr << PVAR(v1.estimate().matrix()) << endl << endl;
          cerr << PVAR(v2.estimate().matrix()) << endl << endl;
          cerr << PVAR(e.measurement().matrix()) << endl << endl;
          return 1;
        }
      }
    }
    cerr << "+";

  }
  return 0;
}

Isometry3D randomIsometry3d

(

)

Definition at line 39 of file test_slam3d_jacobian.cpp.

Referenced by main().

{
  Vector3D rotAxisAngle = Vector3D::Random();
  rotAxisAngle += Vector3D::Random();
  Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized());
  Isometry3D result = (Isometry3D)rotation.toRotationMatrix();
  result.translation() = Vector3D::Random();
  return result;
}