32 #ifndef RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
33 #define RTAB_G2O_EDGE_SBACAM_GRAVITY_H_
35 #include "g2o/types/sba/types_sba.h"
36 #include "g2o/core/base_unary_edge.h"
42 class EdgeSBACamGravity :
public g2o::BaseUnaryEdge<3, Eigen::Matrix<double, 6, 1>, g2o::VertexCam> {
46 information().setIdentity();
48 virtual bool read(std::istream& is) {
return false;}
49 virtual bool write(std::ostream& os)
const {
return false;}
58 const g2o::VertexCam*
v1 =
static_cast<const g2o::VertexCam*
>(_vertices[0]);
60 Eigen::Vector3d direction = _measurement.head<3>();
61 Eigen::Vector3d measurement = _measurement.tail<3>();
67 ea[0] =
atan2(
t (2, 1),
t (2, 2));
68 ea[1] =
asin(-
t (2, 0));
69 ea[2] =
atan2(
t (1, 0),
t (0, 0));
75 Eigen::Vector3d estimate =
rot * -direction;
76 _error = estimate - measurement;
88 _measurement.head<3>() =
m.head<3>().normalized();
89 _measurement.tail<3>() =
m.tail<3>().normalized();