34 #ifndef G2O_SIX_DOF_TYPES_EXPMAP 35 #define G2O_SIX_DOF_TYPES_EXPMAP 37 #include "../core/base_vertex.h" 38 #include "../core/base_binary_edge.h" 39 #include "../core/base_unary_edge.h" 43 #include <Eigen/Geometry> 46 namespace types_six_dof_expmap {
50 using namespace Eigen;
61 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 bool read(std::istream& is);
67 bool write(std::ostream& os)
const;
74 Eigen::Map<const Vector6d>
update(update_);
82 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86 bool read(std::istream& is);
88 bool write(std::ostream& os)
const;
93 Vector2d obs(_measurement);
104 virtual void linearizeOplus();
106 Vector2d cam_project(
const Vector3d & trans_xyz)
const;
108 double fx,
fy, cx, cy;
114 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
118 bool read(std::istream& is);
120 bool write(std::ostream& os)
const;
125 Vector3d obs(_measurement);
136 virtual void linearizeOplus();
138 Vector3d cam_project(
const Vector3d & trans_xyz,
const float &bf)
const;
140 double fx,
fy, cx, cy, bf;
145 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
149 bool read(std::istream& is);
151 bool write(std::ostream& os)
const;
155 Vector2d obs(_measurement);
165 virtual void linearizeOplus();
167 Vector2d cam_project(
const Vector3d & trans_xyz)
const;
170 double fx,
fy, cx, cy;
176 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 bool read(std::istream& is);
182 bool write(std::ostream& os)
const;
186 Vector3d obs(_measurement);
187 _error = obs - cam_project(v1->
estimate().
map(Xw));
196 virtual void linearizeOplus();
198 Vector3d cam_project(
const Vector3d & trans_xyz)
const;
201 double fx,
fy, cx, cy, bf;
virtual void setToOriginImpl()
sets the node to the origin (used in the multilevel stuff)
virtual void oplusImpl(const double *update_)
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential ...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeStereoSE3ProjectXYZOnlyPose()
static SE3Quat exp(const Vector6d &update)
Matrix< double, 6, 6 > Matrix6d
const EstimateType & estimate() const
return the current estimate of the vertex
Vector3d map(const Vector3d &xyz) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EdgeSE3ProjectXYZOnlyPose()