33 #ifndef RTAB_G2O_EDGE_SE3_GRAVITY_H_
34 #define RTAB_G2O_EDGE_SE3_GRAVITY_H_
36 #include "g2o/core/base_unary_edge.h"
37 #include "g2o/types/slam3d/vertex_se3.h"
44 class EdgeSE3Gravity :
public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexSE3> {
48 information().setIdentity();
50 virtual bool read(std::istream& is) {
return false;}
51 virtual bool write(std::ostream& os)
const {
return false;}
55 const g2o::VertexSE3*
v1 =
static_cast<const g2o::VertexSE3*
>(_vertices[0]);
57 Eigen::Vector3d direction = _measurement.head<3>();
58 Eigen::Vector3d measurement = _measurement.tail<3>();
62 Eigen::Matrix3d
t =
v1->estimate().linear();
63 ea[0] =
atan2(
t (2, 1),
t (2, 2));
64 ea[1] =
asin(-
t (2, 0));
65 ea[2] =
atan2(
t (1, 0),
t (0, 0));
71 Eigen::Vector3d estimate =
rot * -direction;
72 _error = estimate - measurement;
84 _measurement.head<3>() =
m.head<3>().normalized();
85 _measurement.tail<3>() =
m.tail<3>().normalized();