首页 > 编程知识 正文

hikaricp源码分析,g2o图优化

时间:2023-05-05 01:28:19 阅读:139442 作者:1767

g2o源代码分析的BaseEdge

BaseUnaryEdge、BaseBinaryEdge、BaseMultiEdge分别表示1边、2边,多边继承自BaseEdgeD、e,

# ifndefg 2o _ base _ binary _ edge _ h # defineg 2o _ base _ binary _ edge _ h # include iostream # include limits # inclincled template int D,typename E,typename VertexXi,typename VertexXj是BaseEdge拥有的

d是int型,表示测定值的维数(dimension ) e表示测定值的数据类型BaseBinaryEdge )双边的新的边缘链接的顶点类型

VertexXi、VertexXj分别是不同的顶点类型template 使用模板的目的就是能够让程序员编写与类型无关的代码例如,如果编写了交换两个整数类型的int类型的swap函数,则只能实现int类型。 无法实现double、字符等类型。 要实现这些类型的交换,必须重新创建另一个swap函数。 使用模板的目的是使此程序的实现不依赖于类型。 例如,swap模板函数可以交换int和double类型。 模板可以应用于函数和类。 ——私人教程

classbasebinaryedge 3360 publicbaseedged,e { public : typedefvertexxivertexxxitype; typedef VertexXj VertexXjType; staticconstintdi=vertexxitype :3360 dimension; staticconstintdj=vertexxjtype :3360 dimension; staticconstintdimension=base edged,E:Dimension; 类型基础边缘,e :3360测量测量; typedef typename Matrixdouble,d,di : alignedmaptypejacobianxioplustype; typedef typename Matrixdouble,d,DJ : alignedmaptypejacobianxjoplustype; typedef typename BaseEdgeD,e :3360 errorvectorerrorvector; typedef typename BaseEdgeD,e :3360 informationtypeinformationtype; typedef eigen :3360 mapmatrixdouble,Di,Dj,matrixdouble,Di,Dj:Flags AlignedBit? aligned : unalignedhessianblocktype; typedef eigen :3360 mapmatrixdouble,Dj,Di,matrixdouble,Dj,Di:Flags AlignedBit? aligned : unalignedhessianblocktransposedtype; basebinaryedge(:baseedged,e )、_hessian(0rowmajor ) false )、_hessian ) 0、vertexxxitype : dimension、 vertex itype 3360//hackwemaptothenullpointerforinitializingthemaps _ hessian transposed (0,VertexXjType:Dimension vertexxitype 3360 _ jacobianoplusxi (0,d,Di )、_jacobianOplusXj(0) 0,d,Dj ) _vertices.resize ) )2); } virtualoptimizablegraph : vertex * create from (; virtualoptimizablegraph : vertex * create to (; virtualvoidresize(s

ize_t size);// typedef unsigned long size_t,why using unsigned long virtual bool allVerticesFixed() const; virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace); /** * Linearizes the oplus operator in the vertex, and stores * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj */ virtual void linearizeOplus(); //! returns the result of the linearization in the manifold space for the node xi const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;} //! returns the result of the linearization in the manifold space for the node xj const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;}

我们这里更新的雅可比矩阵_jacobianOplusXi,_jacobianOplusXj。

virtual void constructQuadraticForm() ; virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor); using BaseEdge<D,E>::resize; using BaseEdge<D,E>::computeError; protected: using BaseEdge<D,E>::_measurement; using BaseEdge<D,E>::_information; using BaseEdge<D,E>::_error; using BaseEdge<D,E>::_vertices; using BaseEdge<D,E>::_dimension; bool _hessianRowMajor; HessianBlockType _hessian; HessianBlockTransposedType _hessianTransposed; JacobianXiOplusType _jacobianOplusXi; JacobianXjOplusType _jacobianOplusXj; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW };#include "base_binary_edge.hpp"} // end namespace g2o#endif

用边表示三维点投影到图像平面的重投影误差(cam_map 函数,.map)

BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>

二元边,第1个2是说测量值是2维的,也就是图像像素坐标x,y的差值,对应测量值的类型是Vector2D,两个顶点也就是优化变量分别是空间点位置 VertexSBAPointXYZ,和李代数位姿VertexSE3Expmap ——渐无书xh

用边表示三维平面投影到相机坐标的误差项

BaseBinaryEdge<3, Eigen::Vector3d, VertexQuatPlane, VertexSE3Expmap>

第一个3表示我们的测量值是三维的,第二个Eigen::Vector3d表示测量数据的数据类型是Eigen::Vector3d,三维向量的形式,我们自定义一个面特征的节点VertexQuatPlane(顶点里主要复写顶点更新函数oplusImpl和顶点重置函数setToOriginImpl),相机节点使用的李代数位姿 VertexSE3Expmap。

我们要自定义边,主要复写 computeError函数:非常重要,是使用当前顶点的值计算的测量值与真实的测量值之间的误差;linearizeOplus函数:非常重要,是在当前顶点的值下,该误差对优化变量的偏导数,也就是我们说的Jacobian。

class EdgeSE3ProjectPlaneSO3: public BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexQuatPlane, g2o::VertexSE3Expmap>{public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // 默认初始化 EdgeSE3ProjectPlaneSO3() { } virtual bool read(istream& in) {} virtual bool write(ostream& out) const {} virtual void computeError() override { // ... _error = _measurement - Something; } virtual void linearizeOplus() override { _jacobianOplusXi(pos, pos) = something; _jacobianOplusXj(pos, pos) = something; } private:};

版权声明:该文观点仅代表作者本人。处理文章:请发送邮件至 三1五14八八95#扣扣.com 举报,一经查实,本站将立刻删除。