13 #include "g2o/types/slam3d/vertex_se3.h"
14 #include "g2o/types/slam3d/isometry3d_gradients.h"
18 using namespace Eigen;
25 _jacobianOplus.clear();
26 _jacobianOplus.push_back(JacobianType(0, 6, 6));
27 _jacobianOplus.push_back(JacobianType(0, 6, 6));
28 _jacobianOplus.push_back(JacobianType(0, 6, 1));
67 g2o::VertexSE3* from =
static_cast<g2o::VertexSE3*
>(_vertices[0]);
68 g2o::VertexSE3* to =
static_cast<g2o::VertexSE3*
>(_vertices[1]);
75 g2o::internal::computeEdgeSE3Gradient(
E, _jacobianOplus[0], _jacobianOplus[1],
Z, Xi, Xj);
77 _jacobianOplus[0]*=vSwitch->estimate();
78 _jacobianOplus[1]*=vSwitch->estimate();
81 _jacobianOplus[2].setZero();
82 _jacobianOplus[2] = g2o::internal::toVectorMQT(
E) * vSwitch->
gradient();
89 const g2o::VertexSE3*
v1 =
dynamic_cast<const g2o::VertexSE3*
>(_vertices[0]);
90 const g2o::VertexSE3*
v2 =
dynamic_cast<const g2o::VertexSE3*
>(_vertices[1]);
94 _error = g2o::internal::toVectorMQT(
delta) *
v3->estimate();