Go to the documentation of this file.
31 #ifndef G2O_EDGE_XY_PRIOR_H
32 #define G2O_EDGE_XY_PRIOR_H
34 #include "g2o/types/slam2d/vertex_point_xy.h"
35 #include "g2o/config.h"
36 #include "g2o/core/base_unary_edge.h"
40 using namespace Eigen;
42 class EdgeXYPrior :
public BaseUnaryEdge<2, Vector2d, VertexPointXY>
50 const VertexPointXY*
v =
static_cast<const VertexPointXY*
>(_vertices[0]);
51 _error =
v->estimate()-_measurement;
53 virtual bool read(std::istream& is);
54 virtual bool write(std::ostream& os)
const;
61 _measurement=Vector2d(
d[0],
d[1]);
74 const VertexPointXY*
v =
static_cast<const VertexPointXY*
>(_vertices[0]);
75 _measurement =
v->estimate();
81 #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
82 virtual void linearizeOplus();
87 BaseUnaryEdge<2, Vector2d, VertexPointXY>()
89 _information.setIdentity();
98 for (
int i = 0;
i < 2; ++
i)
99 for (
int j =
i;
j < 2; ++
j) {
100 is >> information()(
i,
j);
102 information()(
j,
i) = information()(
i,
j);
109 Vector2d
p = measurement();
110 os <<
p.x() <<
" " <<
p.y();
111 for (
int i = 0;
i < 2; ++
i)
112 for (
int j =
i;
j < 2; ++
j)
113 os <<
" " << information()(
i,
j);
118 #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
121 _jacobianOplusXi=Matrix2d::Identity();
virtual bool setMeasurementData(const double *d)
virtual void setMeasurement(const Vector2d &m)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeXYPrior()
virtual void linearizeOplus()
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
ofstream os("timeSchurFactors.csv")
virtual bool getMeasurementData(double *d) const
virtual bool read(std::istream &is)
#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual bool setMeasurementFromState()
virtual bool write(std::ostream &os) const
Array< int, Dynamic, 1 > v
virtual int measurementDimension() const
rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:09