32 #ifndef G2O_EDGE_XYZ_PRIOR_H_
33 #define G2O_EDGE_XYZ_PRIOR_H_
35 #include "g2o/core/base_unary_edge.h"
36 #include "g2o/types/slam3d/vertex_pointxyz.h"
40 using namespace Eigen;
48 class EdgeXYZPrior :
public BaseUnaryEdge<3, Vector3d, VertexPointXYZ> {
52 virtual bool read(std::istream& is);
53 virtual bool write(std::ostream&
os)
const;
58 virtual void linearizeOplus();
78 virtual bool setMeasurementFromState() ;
81 OptimizableGraph::Vertex* ) {
87 information().setIdentity();
93 for (
int i=0; i<3; i++) is >> meas[
i];
97 for (
int i=0;
i<information().rows();
i++)
98 for (
int j=
i;
j<information().cols();
j++){
99 is >> information()(
i,
j);
101 information()(
j,
i)=information()(
i,
j);
108 for (
int i = 0;
i<3;
i++)
os << measurement()[
i] <<
" ";
109 for (
int i=0;
i<information().rows();
i++)
110 for (
int j=
i;
j<information().cols();
j++) {
111 os << information()(
i,
j) <<
" ";
117 const VertexPointXYZ*
v =
static_cast<const VertexPointXYZ*
>(_vertices[0]);
118 _error =
v->estimate() - _measurement;
122 _jacobianOplusXi = Matrix3d::Identity();
126 const VertexPointXYZ*
v =
static_cast<const VertexPointXYZ*
>(_vertices[0]);
127 _measurement =
v->estimate();