test_mat2quat_jacobian.cpp File Reference

#include <iostream>
#include "dquat2mat.h"
#include "isometry3d_mappings.h"
#include "g2o/stuff/macros.h"
#include "EXTERNAL/ceres/autodiff.h"
#include <cstdio>

Include dependency graph for test_mat2quat_jacobian.cpp:

Go to the source code of this file.

Classes
struct	RotationMatrix2QuaternionManifold
	Functor used to compute the Jacobian via AD. More...

Functions
int	main (int, char **)

Function Documentation

int main	(	int	,
		char **
	)

Definition at line 86 of file test_mat2quat_jacobian.cpp.

References g2o::internal::compute_dq_dR().

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

    // create a random rotation matrix by sampling a random 3d vector
    // that will be used in axis-angle representation to create the matrix
    Vector3D rotAxisAngle = Vector3D::Random();
    rotAxisAngle += Vector3D::Random();
    Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized());
    Matrix3D Re = rotation.toRotationMatrix();

    // our analytic function which we want to evaluate
    Matrix<double, 3, 9, Eigen::ColMajor>  dq_dR;
    compute_dq_dR (dq_dR, 
        Re(0,0),Re(1,0),Re(2,0),
        Re(0,1),Re(1,1),Re(2,1),
        Re(0,2),Re(1,2),Re(2,2));

    // compute the Jacobian using AD
    Matrix<double, 3, 9, Eigen::RowMajor> dq_dR_AD;
    typedef ceres::internal::AutoDiff<RotationMatrix2QuaternionManifold, double, 9> AutoDiff_Dq_DR;
    double *parameters[] = { Re.data() };
    double *jacobians[] = { dq_dR_AD.data() };
    double value[3];
    RotationMatrix2QuaternionManifold rot2quat;
    AutoDiff_Dq_DR::Differentiate(rot2quat, parameters, 3, value, jacobians);

    // compare the two Jacobians
    const double allowedDifference = 1e-6;
    for (int i = 0; i < dq_dR.rows(); ++i) {
      for (int j = 0; j < dq_dR.cols(); ++j) {
        double d = fabs(dq_dR_AD(i,j) - dq_dR(i,j));
        if (d > allowedDifference) {
          cerr << "\ndetected difference in the Jacobians" << endl;
          cerr << PVAR(Re) << endl << endl;
          cerr << PVAR(dq_dR_AD) << endl << endl;
          cerr << PVAR(dq_dR) << endl << endl;
          return 1;
        }
      }
    }
    cerr << "+";

  }
  return 0;
}