31 #ifndef G2O_SEVEN_DOF_EXPMAP_TYPES 32 #define G2O_SEVEN_DOF_EXPMAP_TYPES 34 #include "../core/base_vertex.h" 35 #include "../core/base_binary_edge.h" 41 using namespace Eigen;
51 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
53 virtual bool read(std::istream& is);
54 virtual bool write(std::ostream& os)
const;
62 Eigen::Map<Vector7d>
update(const_cast<double*>(update_));
68 setEstimate(s*estimate());
77 res[0] = v[0]*_focal_length1[0] + _principle_point1[0];
78 res[1] = v[1]*_focal_length1[1] + _principle_point1[1];
85 res[0] = v[0]*_focal_length2[0] + _principle_point2[0];
86 res[1] = v[1]*_focal_length2[1] + _principle_point2[1];
102 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
104 virtual bool read(std::istream& is);
105 virtual bool write(std::ostream& os)
const;
111 Sim3 C(_measurement);
113 _error = error_.
log();
121 if (from.count(v1) > 0)
133 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
135 virtual bool read(std::istream& is);
136 virtual bool write(std::ostream& os)
const;
143 Vector2d obs(_measurement);
155 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
157 virtual bool read(std::istream& is);
158 virtual bool write(std::ostream& os)
const;
165 Vector2d obs(_measurement);
Vector2d cam_map1(const Vector2d &v) const
std::set< Vertex * > VertexSet
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
virtual void initialEstimate(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *)
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &, OptimizableGraph::Vertex *)
virtual void oplusImpl(const double *update_)
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)
Vector2d _principle_point2
Sim3 Vertex, (x,y,z,qw,qx,qy,qz) the parameterization for the increments constructed is a 7d vector (...
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
A general case Vertex for optimization.
Vector2d project(const Vector3d &)
7D edge between two Vertex7
const EstimateType & estimate() const
return the current estimate of the vertex
Vector3d map(const Vector3d &xyz) const
virtual void setToOriginImpl()
sets the node to the origin (used in the multilevel stuff)
Vector2d cam_map2(const Vector2d &v) const