微信分销小程序开发,新乡百度网站优化排名,wordpress装修公司模板,seo整站优化新站快速排名G2O整理
整体内容参考#xff1a; 从零开始一起学习SLAM | 理解图优化#xff0c;一步步带你看懂g2o代码 从零开始一起学习SLAM | 掌握g2o顶点编程套路 从零开始一起学习SLAM | 掌握g2o边的代码套路 什么是图优化
什么是图#xff1f;
图优化里的图就是数据结构里的图 从零开始一起学习SLAM | 理解图优化一步步带你看懂g2o代码 从零开始一起学习SLAM | 掌握g2o顶点编程套路 从零开始一起学习SLAM | 掌握g2o边的代码套路 什么是图优化
什么是图
图优化里的图就是数据结构里的图一个图由若干个顶点vertex以及连接这些顶点的边edge组成给你举个例子
比如一个机器人在房屋里移动它在某个时刻 t 的位姿pose就是一个顶点这个也是待优化的变量。而位姿之间的关系就构成了一个边比如时刻 t 和时刻 t1 之间的相对位姿变换矩阵就是边边通常表示误差项。
在SLAM里图优化一般分解为两个任务
1、构建图。机器人位姿作为顶点位姿间关系作为边。
2、优化图。调整机器人的位姿顶点来尽量满足边的约束使得误差最小。 PNP中图优化的例子
G2O整体结构图 1. 图的核心
SparseOptimizer是整个图的核心注意右上角的 is-a 实心箭头这个SparseOptimizer它是一个Optimizable Graph从而也是一个超图HyperGraph
暂时只需要了解一下它们的名字有些以后用不到有些以后用到了再回看。
2. 顶点和边
注意看 has-many 箭头你看这个超图包含了许多顶点HyperGraph::Vertex和边HyperGraph::Edge。而这些顶点继承自 Base Vertex也就是OptimizableGraph::Vertex而边可以继承自 BaseUnaryEdge单边, BaseBinaryEdge双边或BaseMultiEdge多边它们都叫做OptimizableGraph::Edge
3. 配置SparseOptimizer的优化算法和求解器
整个图的核心SparseOptimizer 包含一个优化算法OptimizationAlgorithm的对象。OptimizationAlgorithm是通过OptimizationWithHessian 来实现的。其中迭代策略可以从Gauss-Newton高斯牛顿法简称GN, Levernberg-Marquardt简称LM法, Powell’s dogleg 三者中间选择一个我们常用的是GN和LM
4. 求解
OptimizationWithHessian 内部包含一个求解器Solver这个Solver实际是由一个BlockSolver组成的。这个BlockSolver有两个部分一个是SparseBlockMatrix 用于计算稀疏的雅可比和Hessian矩阵一个是线性方程的求解器LinearSolver它用于计算迭代过程中最关键的一步HΔx−bLinearSolver有几种方法可以选择PCG, CSparse, Choldmod
编程顺序
采用十四讲中g2o求解曲线参数的例子来说明 代码
typedef g2o::BlockSolver g2o::BlockSolverTraits3,1 Block; // 每个误差项优化变量维度为3误差值维度为1// 第1步创建一个线性求解器LinearSolver
Block::LinearSolverType* linearSolver new g2o::LinearSolverDenseBlock::PoseMatrixType(); // 第2步创建BlockSolver。并用上面定义的线性求解器初始化
Block* solver_ptr new Block( linearSolver ); // 第3步创建总求解器solver。并从GN, LM, DogLeg 中选一个再用上述块求解器BlockSolver初始化
g2o::OptimizationAlgorithmLevenberg* solver new g2o::OptimizationAlgorithmLevenberg( solver_ptr );// 第4步创建终极大boss 稀疏优化器SparseOptimizer
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm( solver ); // 设置求解器
optimizer.setVerbose( true ); // 打开调试输出// 第5步定义图的顶点和边。并添加到SparseOptimizer中
CurveFittingVertex* v new CurveFittingVertex(); //往图中增加顶点
v-setEstimate( Eigen::Vector3d(0,0,0) );
v-setId(0);
optimizer.addVertex( v );
for ( int i0; iN; i ) // 往图中增加边
{CurveFittingEdge* edge new CurveFittingEdge( x_data[i] );edge-setId(i);edge-setVertex( 0, v ); // 设置连接的顶点edge-setMeasurement( y_data[i] ); // 观测数值edge-setInformation( Eigen::Matrixdouble,1,1::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵协方差矩阵之逆optimizer.addEdge( edge );
}// 第6步设置优化参数开始执行优化
optimizer.initializeOptimization();
optimizer.optimize(100);求解步骤
1. 创建线性求解器LinearSolver
我们要求的增量方程的形式是H△X-b通常情况下想到的方法就是直接求逆也就是△X-H.inv*b。看起来好像很简单但这有个前提就是H的维度较小此时只需要矩阵的求逆就能解决问题。但是当H的维度较大时矩阵求逆变得很困难求解问题也变得很复杂。
G2O上的求解方法总结
LinearSolverCholmod 使用sparse cholesky分解法。继承自LinearSolverCCSLinearSolverCSparse使用CSparse法。继承自LinearSolverCCSLinearSolverPCG 使用preconditioned conjugate gradient 法继承自LinearSolverLinearSolverDense 使用dense cholesky分解法。继承自LinearSolverLinearSolverEigen 依赖项只有eigen使用eigen中sparse Cholesky 求解因此编译好后可以方便的在其他地方使用性能和CSparse差不多。继承自LinearSolver
2. 创建BlockSolver 并用上面定义的线性求解器初始化
BlockSolver 内部包含 LinearSolver用上面我们定义的线性求解器LinearSolver来初始化。
BlockSolver有两种定义方式
一种是指定的固定变量的solver我们来看一下定义 using BlockSolverPL BlockSolver BlockSolverTraitsp, l ;
其中p代表pose的维度注意一定是流形manifold下的最小表示l表示landmark的维度
另一种是可变尺寸的solver定义如下
using BlockSolverX BlockSolverPLEigen::Dynamic, Eigen::Dynamic;
比较常用的几种类型
BlockSolver_6_3 表示pose 是6维观测点是3维。用于3D SLAM中的BABlockSolver_7_3在BlockSolver_6_3 的基础上多了一个scaleBlockSolver_3_2表示pose 是3维观测点是2维
3. 创建总求解器solver。并从GN, LM, DogLeg 中选一个再用上述块求解器BlockSolver初始化
Solver的优化方法有三种分别是高斯牛顿GaussNewton法LMLevenberg–Marquardt法、Dogleg法
GN、 LM、 Doglet算法内部会发现他们都继承自同一个类OptimizationWithHessian
OptimizationAlgorithmWithHessian发现它又继承自OptimizationAlgorithm
总之在该阶段我们可以选则三种方法
g2o::OptimizationAlgorithmGaussNewton
g2o::OptimizationAlgorithmLevenberg
g2o::OptimizationAlgorithmDogleg 4. 创建终极大boss 稀疏优化器SparseOptimizer并用已定义求解器作为求解方法。
创建稀疏优化器
g2o::SparseOptimizer optimizer;
用前面定义好的求解器作为求解方法
SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
其中setVerbose是设置优化过程输出信息用的
SparseOptimizer::setVerbose(bool verbose)
5. 定义图的顶点和边。 添加到SparseOptimizer中 最重要的一步
顶点Vertex) 从哪里来的 一步步来看吧。先来看看上图中和vertex有关的第①个类 HyperGraph::Vertex在g2o的GitHub上https://github.com/RainerKuemmerle/g2o它在这个路径
g2o/core/hyper_graph.h
这个 HyperGraph::Vertex 是个abstract vertex必须通过派生来使用。如下图所示 然后我们看g2o 类结构图中第②个类我们看到HyperGraph::Vertex 是通过类OptimizableGraph 来继承的 而OptimizableGraph的定义在
g2o/core/optimizable_graph.h
我们找到vertex定义发现果然OptimizableGraph 继承自 HyperGraph如下图所示 不过这个OptimizableGraph::Vertex 也非常底层具体使用时一般都会进行扩展因此g2o中提供了一个比较通用的适合大部分情况的模板。就是g2o 类结构图中 对应的第③个类
BaseVertex
那么它在哪里呢 在这个路径
g2o/core/base_vertex.h BaseVertex-OptimizableGraph-HyperGraph::Vertex
继承关系是HyperGraph::Vertex继承OptimizableGraph继承BaseVertex
顶点Vertex) 参数如何理解
我们来看一下模板参数 D 和 T翻译一下上图红框
D是int 类型的表示vertex的最小维度比如3D空间中旋转是3维的那么这里 D 3
T是待估计vertex的数据类型比如用四元数表达三维旋转的话T就是Quaternion 类型
static const int Dimension D; /// dimension of the estimate (minimal) in the manifold space
可以看到这个D并非是顶点更确切的说是状态变量的维度而是其在流形空间manifold的最小表示这里一定要区别开另外源码里面也给出了T的作用
typedef T EstimateType;
EstimateType _estimate;可以看到这里T就是顶点状态变量的类型
如何自己定义顶点
顶点的基本类型是 BaseVertex那么下一步关心的就是如何使用了因为在不同的应用场景二维空间三维空间有不同的待优化变量位姿空间点还涉及不同的优化类型李代数位姿、李群位姿
g2o本身内部定义了一些常用的顶点类型
VertexSE2 : public BaseVertex3, SE2 //2D pose Vertex, (x,y,theta)
VertexSE3 : public BaseVertex6, Isometry3 //6d vector (x,y,z,qx,qy,qz) 注意我们省略了四元数的w部分
VertexPointXY : public BaseVertex2, Vector2
VertexPointXYZ : public BaseVertex3, Vector3
VertexSBAPointXYZ : public BaseVertex3, Vector3// SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
//SE3顶点内部用变换矩阵参数化外部用指数映射参数化
VertexSE3Expmap : public BaseVertex6, SE3Quat// SBACam Vertex, (x,y,z,qw,qx,qy,qz),(x,y,z,qx,qy,qz) 注意我们省略了四元数的w部分
//假设qw为正否则qx、qy、qz作为旋转存在歧义
VertexCam : public BaseVertex6, SBACam// Sim3 Vertex, (x,y,z,qw,qx,qy,qz),7d vector,(x,y,z,qx,qy,qz) 注意我们省略了四元数的w部分
VertexSim3Expmap : public BaseVertex7, Sim3当然我们可以直接用这些但是有时候我们需要的顶点类型这里面没有就得自己定义了。
重新定义顶点一般需要考虑重写如下函数
virtual bool read(std::istream is);
virtual bool write(std::ostream os) const;
virtual void oplusImpl(const number_t* update);
virtual void setToOriginImpl();这几个是主要要改的地方。我们来看一下他们都是什么意义
readwrite分别是读盘、存盘函数一般情况下不需要进行读/写操作的话仅仅声明一下就可以
setToOriginImpl顶点重置函数设定被优化变量的原始值。
oplusImpl顶点更新函数。非常重要的一个函数主要用于优化过程中增量△x 的计算。我们根据增量方程计算出增量之后就是通过这个函数对估计值进行调整的因此这个函数的内容一定要重视。
自己定义 顶点一般是下面的格式 当我们使用Eigen时在类中需要重载内存分配的new delete函数时加上这句话即可EIGEN_MAKE_ALIGNED_OPERATOR_NEW 比如使用g2o优化时定义一个顶点需要使用eigen中的一些底层函数 class myVertex: public g2::BaseVertexDim, Type{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用时加上此句等于采用eigen中约定的方式重载了该类的new delete等内存分配函数myVertex(){}virtual void read(std::istream is) {}virtual void write(std::ostream os) const {}virtual void setOriginImpl(){_estimate Type();}virtual void oplusImpl(const double* update) override{_estimate /*update*/;}}先看一个简单例子来自十四讲中的曲线拟合来源如下
ch6/g2o_curve_fitting/main.cpp
// 曲线模型的顶点模板参数优化变量维度和数据类型
class CurveFittingVertex: public g2o::BaseVertex3, Eigen::Vector3d
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWvirtual void setToOriginImpl() // 重置{_estimate 0,0,0;}virtual void oplusImpl( const double* update ) // 更新{_estimate Eigen::Vector3d(update);}// 存盘和读盘留空virtual bool read( istream in ) {}virtual bool write( ostream out ) const {}
};我们可以看到下面代码中顶点初值设置为0更新时也是直接把更新量 update 加上去的知道为什么吗
小白更新不就是 x △x 吗这是定义吧
师兄嗯对于这个例子是可以直接加因为顶点类型是Eigen::Vector3d属于向量是可以通过加法来更新的。但是但是有些例子就不行比如下面这个复杂点例子李代数表示位姿VertexSE3Expmap
来自g2o官网在这里
g2o/types/sba/types_six_dof_expmap.h
/**\* \brief SE3 Vertex parameterized internally with a transformation matrixand externally with its exponential map*/class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex6, SE3Quat{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWVertexSE3Expmap();bool read(std::istream is);bool write(std::ostream os) const;virtual void setToOriginImpl() {_estimate SE3Quat();}virtual void oplusImpl(const number_t* update_) {Eigen::Mapconst Vector6 update(update_);setEstimate(SE3Quat::exp(update)*estimate()); //更新方式}
};第一个参数6 表示内部存储的优化变量维度这是个6维的李代数
第二个参数是优化变量的类型这里使用了g2o定义的相机位姿类型SE3Quat。
在这里可以具体查看g2o/types/slam3d/se3quat.h
它内部使用了四元数表达旋转然后加上位移来存储位姿同时支持李代数上的运算比如对数映射log函数、李代数上增量update函数等操作
说完了那我现在问你个问题为啥这里更新时没有像上面那样直接加上去
小白这个表示位姿好像是不能直接加的我记得原因有点忘了
师兄嗯是不能直接加原因是变换矩阵不满足加法封闭。那我再问你为什么相机位姿顶点类VertexSE3Expmap使用了李代数表示相机位姿而不是使用旋转矩阵和平移矩阵
其实也是上述原因的拓展这是因为旋转矩阵是有约束的矩阵它必须是正交矩阵且行列式为1。使用它作为优化变量就会引入额外的约束条件从而增大优化的复杂度。而将旋转矩阵通过李群-李代数之间的转换关系转换为李代数表示就可以把位姿估计变成无约束的优化问题求解难度降低。
刚才是位姿的例子下面是三维点的例子空间点位置 VertexPointXYZ维度为3类型是Eigen的Vector3比较简单就不解释了 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex3, Vector3
{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW VertexSBAPointXYZ();virtual bool read(std::istream is);virtual bool write(std::ostream os) const;virtual void setToOriginImpl() {_estimate.fill(0);}virtual void oplusImpl(const number_t* update){Eigen::Mapconst Vector3 v(update);_estimate v;}
};如何向图中添加顶点
往图中增加顶点比较简单我们还是先看看第一个曲线拟合的例子setEstimate(type) 函数来设定初始值setId(int) 定义节点编号 // 往图中增加顶点CurveFittingVertex* v new CurveFittingVertex();v-setEstimate( Eigen::Vector3d(0,0,0) );v-setId(0);optimizer.addVertex( v );这个是添加 VertexSBAPointXYZ 的例子都很容易看懂 int index 1;for ( const Point3f p:points_3d ) // landmarks{g2o::VertexSBAPointXYZ* point new g2o::VertexSBAPointXYZ();point-setId ( index );point-setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );point-setMarginalized ( true ); optimizer.addVertex ( point );}以上为设置顶点以下为边 初步认识g2o的边
上一次我们讲顶点的时候还专门去追根溯源查找顶点类之间的继承关系边其实也是类似的我们在g2o官方GitHub上这些 g2o/g2o/core/hyper_graph.h g2o/g2o/core/optimizable_graph.h g2o/g2o/core/base_edge.h
头文件下就能看到这些继承关系了 BaseUnaryEdgeBaseBinaryEdgeBaseMultiEdge 分别表示一元边两元边多元边。 一元边你可以理解为一条边只连接一个顶点两元边理解为一条边连接两个顶点也就是我们常见的边啦多元边理解为一条边可以连接多个3个以上顶点 下面我们来看看他们的参数有什么区别你看主要就是 几个参数D, E, VertexXi, VertexXj他们的分别代表
D 是 int 型表示测量值的维度 dimension E 表示测量值的数据类型 VertexXiVertexXj 分别表示不同顶点的类型
比如我们用边表示三维点投影到图像平面的重投影误差就可以设置输入参数如下 BaseBinaryEdge2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap首先这个是个二元边。第1个2是说测量值是2维的也就是图像像素坐标x,y的差值对应测量值的类型是Vector2D两个顶点也就是优化变量分别是三维点 VertexSBAPointXYZ和李群位姿VertexSE3Expmap
除了输入参数外定义边我们通常需要复写一些重要的成员函数
顶点里主要复写了顶点更新函数oplusImpl和顶点重置函数setToOriginImpl
virtual bool read(std::istream is);
virtual bool write(std::ostream os) const;
virtual void computeError();
virtual void linearizeOplus();readwrite分别是读盘、存盘函数一般情况下不需要进行读/写操作的话仅仅声明一下就可以 computeError函数非常重要是使用当前顶点的值计算优化的估计值与真实的测量值之间的误差 linearizeOplus函数非常重要是在当前顶点的值下该误差对优化变量的偏导数也就是我们说的Jacobian矩阵
除了上面几个成员函数还有几个重要的成员变量和函数也一并解释一下
_measurement存储观测值_error存储computeError() 函数计算的误差__vertices[]存储顶点信息比如二元边的话_vertices[] 的大小为2存储顺序和调用setVertex(int, vertex) 是设定的int 有关0 或1setId(int)来定义边的编号决定了在H矩阵中的位置setMeasurement(type) 函数来定义观测值setVertex(int, vertex) 来定义顶点setInformation() 来定义协方差矩阵的逆
如何自定义g2o的边 class myEdge: public g2o::BaseBinaryEdgeerrorDim, errorType, Vertex1Type, Vertex2Type{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW//使用时加上此句等于采用eigen中约定的方式重载了该类的new delete等内存分配函数 myEdge(){} 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;// ... /*_jocobianOplusXj(pos, pos) something;...*/} private:// data}先来看一个简单例子地址在 https://github.com/gaoxiang12/slambook/blob/master/ch6/g2o_curve_fitting/main.cpp 这个是个一元边主要是定义误差函数了如下所示你可以发现这个例子基本就是上面例子的一丢丢扩展
// 误差模型 模板参数观测值维度类型连接顶点类型
class CurveFittingEdge: public g2o::BaseUnaryEdge1,double,CurveFittingVertex
{
public:EIGEN_MAKE_ALIGNED_OPERATOR_NEWCurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}// 计算曲线模型误差void computeError(){const CurveFittingVertex* v static_castconst CurveFittingVertex* (_vertices[0]);const Eigen::Vector3d abc v-estimate();_error(0,0) _measurement - std::exp( abc(0,0)*_x*_x abc(1,0)*_x abc(2,0) ) ;}virtual bool read( istream in ) {}virtual bool write( ostream out ) const {}
public:double _x; // x 值 y 值为 _measurement
};下面是一个复杂一点例子3D-2D点的PnP 问题也就是最小化重投影误差问题这个问题非常常见使用最常见的二元边弄懂了这个基本跟边相关的代码也差不多都一通百通了
代码在g2o的GitHub上这个地方可以看到 g2o/types/sba/types_six_dof_expmap.h 这里根据自己理解对代码加了注释方便理解
//继承了BaseBinaryEdge类观测值是2维类型Vector2D,顶点分别是三维点、李群位姿
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public BaseBinaryEdge2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap{public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW;//1. 默认初始化EdgeProjectXYZ2UV();//2. 计算误差void computeError() {//李群相机位姿v1const VertexSE3Expmap* v1 static_castconst VertexSE3Expmap*(_vertices[1]);// 顶点v2const VertexSBAPointXYZ* v2 static_castconst VertexSBAPointXYZ*(_vertices[0]);//相机参数const CameraParameters * cam static_castconst CameraParameters *(parameter(0));//误差计算测量值减去估计值也就是重投影误差obs-cam//估计值计算方法是T*p,得到相机坐标系下坐标然后在利用camera2pixel()函数得到像素坐标。Vector2D obs(_measurement);_error obs-cam-cam_map(v1-estimate().map(v2-estimate()));}//3. 线性增量函数也就是雅克比矩阵J的计算方法virtual void linearizeOplus();//4. 相机参数CameraParameters * _cam; bool read(std::istream is);bool write(std::ostream os) const;
};有一个地方比较难理解
_error obs - cam-cam_map(v1-estimate().map(v2-estimate()));小白我确实看不懂这一句。。 师兄其实就是误差 观测 - 投影
捋捋思路。我们先来看看cam_map 函数它的定义在 g2o/types/sba/types_six_dof_expmap.cpp cam_map 函数功能是把相机坐标系下三维点输入用内参转换为图像坐标输出具体代码如下所示
Vector2 CameraParameters::cam_map(const Vector3 trans_xyz) const {Vector2 proj project2d(trans_xyz);Vector2 res;res[0] proj[0]*focal_length principle_point[0];res[1] proj[1]*focal_length principle_point[1];return res;
}然后看 .map函数它的功能是把世界坐标系下三维点变换到相机坐标系函数在 g2o/types/sim3/sim3.h 具体定义是 Vector3 map (const Vector3 xyz) const {return s*(r*xyz) t;}因此下面这个代码
v1-estimate().map(v2-estimate())就是用V1估计的pose把V2代表的三维点变换到相机坐标系下。
前面主要是对computeError() 的理解还有一个很重要的函数就是linearizeOplus()用来定义雅克比矩阵 我摘取了相关代码来自g2o/g2o/types/sba/types_six_dof_expmap.cpp并进行了标注相信会更容易理解
十四讲第169页中的雅克比矩阵完全是按照书上 式子7.45、7.47来编程的不难理解 小白后面就是直接照抄书上就行哈哈 如何向图中添加边
一元边的添加方法
下面代码来自GitHub上仍然是前面曲线拟合的例子 slambook/ch6/g2o_curve_fitting/main.cpp // 往图中增加边for ( int i0; iN; i ){CurveFittingEdge* edge new CurveFittingEdge( x_data[i] );edge-setId(i); // 设置边的序号edge-setVertex( 0, v ); // 设置连接的顶点edge-setMeasurement( y_data[i] ); // 观测数值edge-setInformation( Eigen::Matrixdouble,1,1::Identity()*1/(w_sigma*w_sigma) ); // 信息矩阵协方差矩阵之逆optimizer.addEdge( edge );}小白setMeasurement 函数的输入的观测值具体是指什么 师兄对于这个曲线拟合观测值就是实际观测到的数据点。对于视觉SLAM来说通常就是我们我们观测到的特征点坐标下面就是一个例子。这个例子比刚才的复杂一点因为它是二元边需要用边连接两个顶点 代码来自GitHub上 slambook/ch7/pose_estimation_3d2d.cpp index 1;for ( const Point2f p:points_2d ){g2o::EdgeProjectXYZ2UV* edge new g2o::EdgeProjectXYZ2UV();edge-setId ( index );edge-setVertex ( 0, dynamic_castg2o::VertexSBAPointXYZ* ( optimizer.vertex ( index ) ) );edge-setVertex ( 1, pose );edge-setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );edge-setParameterId ( 0,0 );edge-setInformation ( Eigen::Matrix2d::Identity() );optimizer.addEdge ( edge );index;}小白这里的setMeasurement函数里的p来自向量points_2d也就是特征点的图像坐标(x,y)了吧 师兄对这正好呼应我刚才说的。另外你看setVertex 有两个一个是 0 和 VertexSBAPointXYZ 类型的顶点一个是1 和pose。你觉得这里的0和1是什么意思能否互换呢
小白01应该是分别指代哪个顶点吧直觉告诉我不能互换可能得去查查顶点定义部分的代码 师兄你的直觉没错我帮你 查过啦你看这个是setVertex在g2o官网的定义
// set the ith vertex on the hyper-edge to the pointer supplied
void setVertex(size_t i, Vertex* v) { assert(i _vertices.size() index out of bounds); _vertices[i]v;}这段代码在 g2o/core/hyper_graph.h 里可以找到。你看 _vertices[i] 里的i就是我们这里的0和1我们再去看看这里边的类型 g2o::EdgeProjectXYZ2UV 的定义前面我们也放出来了就这两句
class G2O_TYPES_SBA_API EdgeProjectXYZ2UV
.....//李群相机位姿v1
const VertexSE3Expmap* v1 static_castconst VertexSE3Expmap*(_vertices[1]);
// 顶点v2
const VertexSBAPointXYZ* v2 static_castconst VertexSBAPointXYZ*(_vertices[0]);你看 _ vertices[0] 对应的是 VertexSBAPointXYZ 类型的顶点也就是三维点 _vertices[1] 对应的是VertexSE3Expmap 类型的顶点也就是位姿pose。
因此前面 1 对应的就应该是 pose0对应的 应该就是三维点。
6. 设置优化参数开始执行优化
设置SparseOptimizer的初始化、迭代次数、保存结果等
初始化
SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet eset)
设置迭代次数开始执行图优化
SparseOptimizer::optimize(int iterations, bool online)
练习题目 题目给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)以及相机的内参矩阵。使用bundle adjustment 方法g2o库实现来估计相机的位姿T。初始位姿T为单位矩阵。
代码框架 链接https://pan.baidu.com/s/1CiSU-8rDBWurk1ZpzMk_Nw 提取码ebcj 实现https://zhuanlan.zhihu.com/p/64587232
