#include <types_icp.h>

Public Member Functions
	EdgeGICP ()

void	makeRot0 ()

void	makeRot1 ()

Matrix3D	prec0 (double e)

Matrix3D	prec1 (double e)

Matrix3D	cov0 (double e)

Matrix3D	cov1 (double e)

Public Attributes
Vector3D	pos0

Vector3D	pos1

Vector3D	normal0

Vector3D	normal1

Matrix3D	R0

Matrix3D	R1

Private Attributes
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW

Detailed Description

Definition at line 60 of file types_icp.h.

Constructor & Destructor Documentation

g2o::EdgeGICP::EdgeGICP ( )

inline

Definition at line 75 of file types_icp.h.

       {
         pos0.setZero();
         pos1.setZero();
         normal0 << 0, 0, 1;
         normal1 << 0, 0, 1;
         //makeRot();
         R0.setIdentity();
         R1.setIdentity();
       }

Member Function Documentation

Matrix3D g2o::EdgeGICP::cov0 ( double e )

inline

Definition at line 136 of file types_icp.h.

     {
       makeRot0();
       Matrix3D cov;
       cov  << 1, 0, 0,
               0, 1, 0,
               0, 0, e;
       return R0.transpose()*cov*R0;
     }

Matrix3D g2o::EdgeGICP::cov1 ( double e )

inline

Definition at line 147 of file types_icp.h.

     {
       makeRot1();
       Matrix3D cov;
       cov  << 1, 0, 0,
               0, 1, 0,
               0, 0, e;
       return R1.transpose()*cov*R1;
     }

void g2o::EdgeGICP::makeRot0 ( )

inline

Definition at line 87 of file types_icp.h.

Referenced by g2o::Edge_V_V_GICP::read().

     {
       Vector3D y;
       y << 0, 1, 0;
       R0.row(2) = normal0;
       y = y - normal0(1)*normal0;
       y.normalize();            // need to check if y is close to 0
       R0.row(1) = y;
       R0.row(0) = normal0.cross(R0.row(1));
       //      cout << normal.transpose() << endl;
       //      cout << R0 << endl << endl;
       //      cout << R0*R0.transpose() << endl << endl;
     }

void g2o::EdgeGICP::makeRot1 ( )

inline

Definition at line 102 of file types_icp.h.

     {
       Vector3D y;
       y << 0, 1, 0;
       R1.row(2) = normal1;
       y = y - normal1(1)*normal1;
       y.normalize();            // need to check if y is close to 0
       R1.row(1) = y;
       R1.row(0) = normal1.cross(R1.row(1));
     }

Matrix3D g2o::EdgeGICP::prec0 ( double e )

inline

Definition at line 114 of file types_icp.h.

Referenced by main().

     {
       makeRot0();
       Matrix3D prec;
       prec << e, 0, 0,
               0, e, 0,
               0, 0, 1;
       return R0.transpose()*prec*R0;
     }

Matrix3D g2o::EdgeGICP::prec1 ( double e )

inline

Definition at line 125 of file types_icp.h.

     {
       makeRot1();
       Matrix3D prec;
       prec << e, 0, 0,
               0, e, 0,
               0, 0, 1;
       return R1.transpose()*prec*R1;
     }