【VSLAM】9——图优化与g2o

Table of Contents

0. 引入

1. 目标函数

2. 非线性优化

3. g2o简介

4. g2o曲线拟合代码解析

5. g2o两图像间PnP方法位姿优化

6. g2o两图像间ICP方法位姿优化


 

 

持续更新中......

参考资料:

深入理解图优化与g2o:图优化篇

从零开始一起学习SLAM | 理解图优化,一步步带你看懂g2o代码

 

0. 引入

图优化本质上也是一个优化问题,是一种将优化问题表现成图的一种方式。

图是由顶点(Vertex)以及连接各个顶点的(Edge)组成,边就是点与点之间的关系。在SLAM中,顶点用来表示优化变量(机器人位姿),边用来表示误差项(位姿之间的关系)。边也可以连接一个顶点(Unary Edge,一元边)、两个顶点(Binary Edge,二元边)或多个顶点(Hyper Edge,多元边)。当一个图中存在连接两个以上顶点的边时,称这个图为超图(Hyper Graph)。而SLAM问题就可以表示成一个超图。

对于任意一个最小二乘问题,我们可以构建与之对应的一个图。在求解时,也可以去掉孤立顶点或者优先优化边数较多(度数较大)的顶点进行改进。

【VSLAM】9——图优化与g2o_第1张图片

可通过如下博客进一步理解图优化:

graph slam tutorial :从推导到应用1

 

1. 目标函数

以下以观测模型为例进行分析,假设共有n个位姿和m个观测点(SLAM中除了观测模型外还有运动模型,不同的SLAM方法中运动模型大体相近)。

假设机器人位姿在\(x_{k}\)处,观测到路标\(y_{j}\),产生观测数据\(z_{kj}\),三者关如下:$$z_{kj}=f(x_{k},y_{j})$$

有测量就会产生误差,假设观测过程产生的噪声(误差)为\(e_{kj}\),则:$$e_{kj}=z_{kj}-f(x_{k},y_{j})$$

式中的\(x_{k}\)、\(y_{j}\)可以只是一个顶点,也可以是多个顶点,取决于边的实际类型(一元边、二元边或者多元边),序号为kj的边根据使用的顶点应表示为\(e_{kj}(z_{kj},x_{k1},x_{k2},...,y_{j1},y_{j2}...)\),简洁的表示\(e_{kj}\)。

多次测量就会差生多个不同的误差,优化的目的是使总的误差得到最小值。若以\(x_{k}\)、\(y_{j}\)为优化变量,此时\(e_{kj}\)为关于\(x_{k}\)、\(y_{j}\)的函数,以\(F(x,y)=\sum _{k=1}^{n} \sum _{j=1}^{m} \left \| e_{kj} \right \|\)为目标函数,则可以计算的到目标函数的最小值,对\(x_{k}\)、\(y_{j}\)进行优化获取估计值。最终的目标函数应表示如下:

$$F(x,y)=\sum _{k=1}^{n} \sum _{j=1}^{m}e_{kj}^{T}\Omega _{kj}e_{kj}$$

下面将通过两种方式对上式进行解释。

(1)方法一:直接法

误差\(e_{kj}\)为一个矢量,但目标函数必须为标量才能求解出最优解,所以可以以使用平方的形式来表示目标函数,最简单的形式为:\(e_{kj}^{T}e_{kj}\)。为了表示误差各分量重视程度的不一样,可以使用一个信息矩阵\(\Omega\)来表示各分量的不一致性,即写为\(e_{kj}^{T}\Omega_{kj} e_{kj}\),所以最后可得:$$F(x,y)=\sum _{k=1}^{n} \sum _{j=1}^{m}e_{kj}^{T}\Omega_{kj}e_{kj}$$

/*End of Method 1*/

(2)方法二:贝叶斯方式

需要了解的相关基础知识:验概率、最大似然估计、贝叶斯估计、最大后验概率

假设噪声项\(e_{kj}\)满足高斯分布\(e_{kj}\sim N(0,Q_{kj})\),则观测数据的最大似然估计为:$$P(z_{kj}|x_{k},y_{j})=N(f(x_{k},y_{j}),Q_{k})$$

已知任意高维高斯分布\(x\sim N(\mu ,\Sigma )\)的概率密度展开式为:$$P(x)=\frac{1}{\sqrt{(2\pi )^{N}det(\Sigma )}}exp(-\frac{1}{2}(x-\mu )^{T}\Sigma ^{-1}(x-u))$$

两边对其取负对数则可得到:$$-ln(P(x))=\frac{1}{2}ln((2\pi )^{N}det(\Sigma ))+\frac{1}{2}(x-\mu )^{T}\Sigma ^{-1}(x-\mu )$$

上式第一项与\(x\)无关,可以忽略,只要最小化右侧的二次型,可得到对状态的最大似然估计,即求出相应的\(x_{k}\),\(y_{j}\)使下式能够取得最小值:$$F_{kj}(x_{k},y_{j})=((z_{kj}-f(x_{k},y_{j}))^{T}Q^{-1}_{kj}(z_{kj}-f(x_{k},y_{j})))$$

将误差带入可得:$$F_{kj}(x_{k},y_{j})=(e_{kj}^{T}Q^{-1}_{kj}e_{kj})$$

将所有的观测与误差数据进行加和,相当于n条边加和的形式,可得到目标函数为:$$F(x,y)=\sum _{k=1}^{n} \sum _{j=1}^{m}e_{kj}^{T}Q^{-1}_{kj}e_{kj}$$

/*End of Method 2*/

以上是观测模型误差。值得注意的是,方法1的\(\Omega\)为协方差矩阵逆,即为信息矩阵,方法2的\(Q\)为协方差矩阵,协防差矩阵为对称矩阵,矩阵中每一个元素都为对应误差的系数,表示为相关性的预计。最简单的是把\(\Omega\)设成对角矩阵,矩阵元素的大小表明对此项误差的重视程度。

 

2. 非线性优化

上面已经给出目标函数,应该如何对目标函数进行优化?

对于目标函数\(f(x)\),若要求其最小值,通常情况下,由于最值点一般也是极值点,所以求出\(f(x)\)所有极值点,然后进行对比就能得到最值,若函数为非线性,可以使用迭代的方式求解:

  1. 给定某个初始值\(x_{0}\);
  2. 寻找一个增量\(\Delta x\),使函数达到极小值;
  3. 若\(\Delta x\)足够小,则停止,否则继续执行step 2。

(1)求增量\(\Delta x\)

根据上述可知,\(e_{kj}\)为关于\(x_{k}\)、\(y_{j}\)的函数,其原型为\(e_{kj}(z_{kj},x_{k1},x_{k2},...,y_{j1},y_{j2}...)\),为了使下面的推导更简洁,将全部变量一起表示为X,误差表示为:$$e_{kj}(X)=z_{kj}-f_{kj}(X)$$

对应的目标函数中的一项可表示为:$$F_{kj}(X)=e_{ij}(X)^{T}\Omega e_{ij}(X)$$

假设其中一条边给定的初始值为\({x}_{0}\),并给予了一个\(\Delta{x}\)的增量,这条边的误差由\(e_{kj}(x_{0})\)变更为\(e_{kj}(x_{0}+\Delta x)\),同时\(F_{kj}({x}_{0})\)变更为\(F_{kj}({x}_{0}+\Delta x)\)。对这条边误差进行一阶泰勒展开可得:$$e_{kj}({x}_{0}+\Delta x)\approx e_{kj}({x}_{0})+J_{kj}\Delta x$$

其中,\(J_{kj}\)为\(e_{kj}\)关于\(x_{0}\)的雅可比矩阵。对\(F_{kj}\)展开可以得到:$$F_{kj}({x}_{0}+\Delta x)=e_{kj}({x}_{0}+\Delta x)^{T}\Omega e_{kj}({x}_{0}+\Delta x)\approx (e_{kj}({x}_{0})+J_{kj}\Delta x)^{T}\Omega (e_{kj}({x}_{0})+J_{kj}\Delta x)=e_{kj}({x}_{0})^{T}\Omega e_{kj}({x}_{0})+2e_{kj}({x}_{0})^{T}\Omega J_{kj}\Delta x+(J_{kj}\Delta x)^{T}\Omega J_{kj}\Delta x=C_{kj}+2b_{kj}\Delta x+\Delta x^{T}H_{kj}\Delta x$$

其中,\(C_{kj}\)是\(\Delta x\)的无关项,\(2b_{kj}\)是\(\Delta x\)的一次项系数,\(H_{kj}\)是\(\Delta x\)的二次项系数,并且:

$$H_{kj}=(J_{kj})^{T}\Omega J_{kj}$$$$b_{kj}=e_{kj}^{T}\Omega J_{kj}$$

最终目标是得到增量\(\Delta x\),使目标函数取极小值,所以应该让目标函数对\(\Delta x\)求导并赋值为0:

$$\frac{dF_{kj}}{d\Delta x}=2b_{kj}+2H_{kj}\Delta x$$

解得:$$H_{kj}\Delta x=-b_{kj}$$

上式为一条边(序号为kj)的求解方程,则所有的边一起考虑,矩阵进行叠加运算,则可以去掉下标:$$H\Delta x=-b$$其中:$$H=\sum_{k}^{}\sum_{j}H_{kj}$$$$b=\sum_{k}^{}\sum_{j}b_{kj}$$

高斯-牛顿法(Guass-Newton)就是不断重复这个过程直到收敛,而列文伯格—马夸尔特法(LM)引入了一个松弛因子来控制迭代速度:$$(H+\lambda I)\Delta x=-b$$

在上述求给\(F_{kj}\)加增量\(\Delta x\)时直接写作了\(F_{kj}({x}_{0}+\Delta x)\),而加法很可能是没有定义的,例如位姿变量通常情况下以变换矩阵的形式表示,但是变换矩阵对加法并不封闭,所以需要使用李代数进行计算,李代数的简介可参考:【VSLAM】2——李群与李代数

(2)求雅可比矩阵\(J\)
上述给出了求解增量\(\Delta x\)的方法,根据式\(H\Delta x=-b\),前提是需要求解出误差关于优化变量的一阶雅可比矩阵,才能进一步求解\(H\)和\(b\)以及最终的\(\Delta x\)。

下面以PnP问题(3d-2d)为例

优化过程中共存在位姿和路标点两个待优化变量,所以需要分别求出对应优化变量的雅可比。回到最开始的公式:$$e_{kj}=z_{kj}-f(x_{k},y_{j})$$在PnP问题中,\(x_{k}\)表示待估计位姿,\(y_{j}\)表示待估计路标点,函数\(f\)为摄像头所理论到路标点在成像平面的投影,\(z_{kj}\)为实际观测值得到的投影值,\(e_{kj}\)为理论与实际投影值之间的误差。误差项将像素坐标与3D坐标按照当前估计的位姿进行投影的到的位置相比较得到的误差,为最小化重投影误差。假设路标点的世界坐标为\(P_{j}=\begin{bmatrix} X & Y & Z \end{bmatrix}^{T}\),相机坐标为\(P’_{j}=\begin{bmatrix} X’ & Y’ & Z’ \end{bmatrix}^{T}\),观测到的像素坐标\(u_{kj}=\begin{bmatrix} u& v \end{bmatrix}^{T}\),位姿\(R\)和\(t\)对应的李代数为\(\xi_{k}\),\(K\)为相机内参,对应公式如下:$$s\begin{bmatrix} u\\v \\1 \end{bmatrix}=K\begin{bmatrix} X’\\Y’ \\Z’ \\1 \end{bmatrix}=Kexp(\hat{\xi_{k}} )\begin{bmatrix} X\\Y \\Z \\1 \end{bmatrix}$$简写为:$$u_{kj}=\frac{1}{s}Kexp(\hat{\xi_{k} })P_{j}$$误差可写为:$$e_{kj}=u_{kj}-\frac{1}{s}Kexp(\hat{\xi_{k} })P_{j}$$

PnP误差关于李代数(位姿)的雅可比如下:$$\frac{\partial e}{\partial \xi}=\frac{\partial e}{\partial P{}'}\frac{\partial P{}'}{\partial \xi}$$第一项误差关于投影点的导数:$$\frac{\partial e}{\partial P{}'}=-\begin{bmatrix} \frac{\partial u}{\partial X{}'} & \frac{\partial u}{\partial Y{}'} & \frac{\partial u}{\partial Z{}'}\\ \frac{\partial v}{\partial X{}'} & \frac{\partial v}{\partial Y{}'} & \frac{\partial v}{\partial Z{}'} \end{bmatrix}=-\begin{bmatrix} \frac{f_{x}}{Z{}'} & 0 & \frac{f_{x}X{}'}{Z{}'^{2}}\\ 0 & \frac{f_{y}}{Z{}'} & \frac{f_{y}Y{}'}{Z{}'^{2}} \end{bmatrix}$$第二项误差关于李代数的导数(可参考:【VSLAM】2——李群与李代数):$$\frac{\partial P'}{\partial \xi}=\begin{bmatrix} -\hat{P'} &I \end{bmatrix}$$即可得到2x6的矩阵:$$\frac{\partial e}{\partial \xi}=-\begin{bmatrix} -\frac{f_{x}X{}'Y{}'}{Z{}'^{2}}&f_{x}+\frac{f_{x}X^{2}}{Z{}'^{2}}&-\frac{f_{x}Y{}'}{Z{}'}&\frac{f_{x}}{Z{}'}&0&\frac{f_{x}X{}'}{Z{}'^{2}}\\-f_{y}-\frac{f_{y}Y{}'^{2}}{Z{}'^{2}}&\frac{f_{y}X{}'Y{}'}{Z{}'^{2}}&\frac{f_{y}X{}'}{Z{}'}&0&\frac{f_{y}}{Z{}'}&\frac{f_{y}Y{}'}{Z{}'^{2}}\end{bmatrix}$$

PnP误差关于路标的雅可比如下:$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P{}'}\frac{\partial P{}'}{\partial P}$$第一项已在上面进行推导,第二项如下:$$\frac{\partial P{}'}{\partial P}=\frac{\partial (RP+t)}{\partial P}=R$$即可得到:$$\frac{\partial e}{\partial P}=-\begin{bmatrix} \frac{f_{x}}{Z{}'} & 0 & \frac{f_{x}X{}'}{Z{}'^{2}}\\ 0 & \frac{f_{y}}{Z{}'} & \frac{f_{y}Y{}'}{Z{}'^{2}} \end{bmatrix}R$$

下面再以ICP问题(3d-3d)为例

ICP的误差可写为:$$e_{kj}=P_{kj}-exp(\hat{\xi_{k}})P_{j}$$在下面的例程中仅对位姿进行了优化,所以这里仅给出关于位姿的雅可比。

ICP误差关于李代数(位姿)的雅可比如下

在PnP中,由于误差是二维的,所以需要通过链式法则进行一次转换;在ICP中误差是三维的,可以直接对李代数进行求导,即可得到3x6的矩阵:$$\frac{\partial e}{\partial \xi}=-\begin{bmatrix} -\hat{P} &I \end{bmatrix}=\begin{bmatrix} 0 & -z & y &-1&0&0\\ z&0&-x&0&-1&0\\-y&x&0&0&0&-1 \end{bmatrix}$$

另外,值得注意的是,在PnP方法中使用的\(R\)和\(t\)为\(R_{cw}\)和\(t_{cw}\),ICP方法中使用的\(R\)和\(t\)为\(R_{wc}\)和\(t_{wc}\)。

 

(3)稀疏性

关于\(H\)和\(J\)稀疏性的理解,可参考:graph slam tutorial :从推导到应用2

 

3. g2o简介

使用g2o可以轻松的使用非线性优化方法对问题进行求解。

g2o基本框架如下图所示:

【VSLAM】9——图优化与g2o_第2张图片

图上半部分:

SparseOptimizer(稀疏优化器)是整个图的核心,其为一个Optimizable Graph(可优化图),也是一个HyperGraph(超图);超图包含了许多HyperGraph::Vertex(顶点)HyperGraph::Edge(边),其中顶点继承自Base Vertex,边继承自 BaseUnaryEdge,BaseBinaryEdge或BaseMultiEdge。Base Vertex和Base Edge都是抽象的基类,而实际用的顶点和边,都是它们的派生类。总结来说,顶点用来储存更新待优化的变量,边用来计算观测产生的误差。SparseOptimizer.addVertex和SparseOptimizer.addEdge向一个图中添加顶点和边,最后调用SparseOptimizer.optimize完成优化。

图下半部分:

SparseOptimizer拥有一个Optimization Algorithm(优化算法),继承自Gauss-Newton,Levernberg-Marquardt,Powell's dogleg三者之一;同时,这个 Optimization Algorithm 拥有一个Solver(求解器),SparseBlockMatrix(稀疏块矩阵)用于计算稀疏的雅可比和海塞,LinearSolver(线性求解器)计算迭代过程中最关键的一步:$$H\Delta x=-b$$

 

4. g2o曲线拟合代码解析

源码地址:https://github.com/gaoxiang12/slambook/edit/master/ch6/g2o_curve_fitting/main.cpp

/*note:
 *曲线拟合问题只有一个顶点,各个带噪声的数据点为误差项。
 */
#include 
#include 
#include 
#include     /*Eigen头文件*/
#include     /*OpenCV头文件*/
#include     /*基础顶点头文件*/
#include     /*基础边头文件*/
#include     /*求解器头文件*/
#include     /*优化算法头文件,包括GN\LM\DL*/
#include 
#include 
#include     /*线性稠密求解器*/

using namespace std; 

/*定义CurveFittingVertex类,继承g2o::BaseVertex类,
 *需要制定相应的模板参数:维度(a,b,c共3维)和类型(Eigen::Vector3d),
 *
 *其中g2o::BaseVertex类继承OptimizableGraph::Vertex类,
 *OptimizableGraph::Vertex类继承HyperGraph::Vertex类和HyperGraph::DataContainer类。
 */
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    /*Eigen对齐*/
    virtual void setToOriginImpl()    /*设置初值函数*/
    {
        _estimate << 0,0,0;    /*待估计的状态变量,BaseVertex类中protected成员变量*/
    }
    virtual void oplusImpl( const double* update )/*更新待估计的状态变量函数*/
    {
        _estimate += Eigen::Vector3d(update);    /*叠加更新x+delta(x)*/
    }
    virtual bool read( istream& in ) {}    /*存盘和读盘:留空*/
    virtual bool write( ostream& out ) const {}
};

/*定义CurveFittingVertex边类,继承g2o::BaseUnaryEdge类(一元边),
 *需要制定相应的模板参数:维度,类型,顶点类型,
 *每条边代表一个观测值,
 *
 *其中g2o::BaseUnaryEdge类继承BaseEdge类,
 *BaseEdge类继承OptimizableGraph::Edge类,
 *OptimizableGraph::Edge类继承HyperGraph::Edge类和HyperGraph::DataContainer类。
 */
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW    /*Eigen对齐*/
    CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}    /*构造函数*/
    void computeError()    /*计算曲线模型误差函数*/
    {
        /*_vertices是HyperGraph::Edge类中protected成员变量,表示边连接的顶点,
         *一元边只有一个顶点,static_cast强制类型转换,赋值给v*/
        const CurveFittingVertex* v = static_cast (_vertices[0]);
        /*调用g2o::BaseVertex类中estimate()方法获得当前估计值_estimate*/
        const Eigen::Vector3d abc = v->estimate();
        /*_error为OptimizableGraph::Edge类中记录误差的变量,
         *其数据类型为Eigen::Matrix,与维度相关,
         *误差由OptimizableGraph::Edge类中测量值_measurement减通过估计值获得的计算值得到
         */
        _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赋值给_x*/
};

int main( int argc, char** argv )
{
    double a=1.0, b=2.0, c=1.0;    /*实际数据*/        
    int N=100;     /*数据点数*/                         
    double w_sigma=1.0;    /*噪声*/                 
    cv::RNG rng;    /*随机数产生器*/                        
    double abc[3] = {0,0,0};    /*估计值*/

    vector x_data, y_data;     /*生成随机观测数据*/
    cout<<"generating data: "< > Block;
    
    /*step1:创建线性方程求解器赋值于linearSolver中*/
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense(); 
    
    /*step2:创建BlockSolver矩阵求解器,使用linearSolver初始化*/
    Block* solver_ptr = new Block( unique_ptr(linearSolver) );      
    
    /*step3:选择总求解器solver,使用BlockSolver初始化*/
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( unique_ptr(solver_ptr) );
    // g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
    // g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
    
    /*step4:创建稀疏优化器SparseOptimizer*/
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );     /*设置求解器*/
    optimizer.setVerbose( true );    /*打开调试输出*/       
    
    /*step5:添加顶点和边*/
    CurveFittingVertex* v = new CurveFittingVertex();
    v->setEstimate( Eigen::Vector3d(0,0,0) );    /*设定顶点的初始状态*/
    v->setId(0);    /*设定当前顶点的id*/
    optimizer.addVertex( v );    /*向构建的图中加入设置好的顶点(只有一个顶点)*/
    
    for ( int i=0; isetId(i);    /*设定当前边的id*/        
        edge->setVertex( 0, v );    /*设定顶点*/         
        edge->setMeasurement( y_data[i] );    /*设定测量值*/ 
        edge->setInformation(Eigen::Matrix::Identity()*1/(w_sigma*w_sigma) ); 
        optimizer.addEdge( edge );
    }
    
    cout<<"start optimization"< time_used = chrono::duration_cast>( t2-t1 );
    cout<<"solve time cost = "<estimate();
    cout<<"estimated model: "<

g2o中选择优化方法一共需要5个步骤:

(1)LinearSolver

增量\(\Delta x=-inverse(H)b\),要求解\(\Delta x\)需要对矩阵\(H\)求逆。LinearSolver可以使用特殊的方法对矩阵进行求逆。在g2o的目录“g2o/g2o/solvers”下可以查看到不同的方法。

LinearSolverCholmod :使用sparse cholesky分解法。继承自LinearSolverCCS;

LinearSolverCSparse:使用CSparse法。继承自LinearSolverCCS;

LinearSolverPCG :使用preconditioned conjugate gradient 法,继承自LinearSolver;

LinearSolverDense :使用dense cholesky分解法。继承自LinearSolver;

LinearSolverEigen: 依赖项只有eigen,使用eigen中sparse Cholesky 求解,因此编译好后可以方便的在其他地方使用,性能和CSparse差不多。继承自LinearSolver。

(2)BlockSolver

BlockSolver 内部包含 LinearSolver,需要使用LinearSolver来初始化,其头文件路径为”g2o/g2o/core/block_solver.h“。

BlockSolver共两种定义形式,第一种为固定变量,其中p为位姿维度,l为路标点的维度:

using BlockSolverPL = BlockSolver< BlockSolverTraits >;

第二种为可变尺寸,在位姿和路标点在程序开始不能确定时使用:

using BlockSolverX = BlockSolverPL

头文件最后还定义了另外三种形式:

  // solver for BA/3D SLAM
  using BlockSolver_6_3 = BlockSolverPL<6, 3>;

  // solver fo BA with scale
  using BlockSolver_7_3 = BlockSolverPL<7, 3>;

  // 2Dof landmarks 3Dof poses
  using BlockSolver_3_2 = BlockSolverPL<3, 2>;

 (3)创建总求解器solver

 总求解器目录位于”g2o/g2o/core“,优化方法有三种:高斯牛顿(GaussNewton)法,LM(Levenberg–Marquardt)法、Dogleg法,都继承自同一个类:OptimizationWithHessian,OptimizationAlgorithmWithHessian又继承自OptimizationAlgorithm。

 (4)创建稀疏优化器SparseOptimizer

创建稀疏优化器optimizer,并使用求解器作为求解方法。

(5)添加顶点和边

每一个待优化的变量都需要使用顶点类定义一个对象,每一次的观测都需要使用边类定义一个对象,并将所定义的顶点和边的对象添加进优化器中。其中每条边都由相对应顶点构成。

顶点使用的变量成员:

_estimate:待优化的变量,每一次优化器的迭代后,变量都会进行相应的更新

边使用的变量成员:

_vertices:存储边对应的顶点,其中一元边存于_vertices[0]中

_measurement:边的观测值,对应上面代码中的y_data[i]

_error:记录误差的变量,类型为Eigen::Matrix的矩阵,所以不能直接赋值double类型的数据。在上面的例程中,需要使用_error(0,0)承接double类型的误差。

更多顶点和边的定义和使用方法可参考:

从零开始一起学习SLAM | 掌握g2o顶点编程套路

从零开始一起学习SLAM | 掌握g2o边的代码套路

(6)初始化稀疏优化器,设置迭代次数上限

 

5. g2o两图像间PnP方法位姿优化

源码地址:https://github.com/gaoxiang12/slambook/blob/master/ch7/pose_estimation_3d2d.cpp

代码的整个流程是,已知世界坐标系特征点的3D坐标(通过三角化得到),已知位姿变换后的对应特征点的2D像素坐标,并使用PnP计算出相应的R和t后,使用BA方法构建最小二乘问题并进行位姿的优化。

void bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse();
    Block* solver_ptr = new Block ( linearSolver );
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );

    /*位姿顶点*/
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at ( 0,0 ), R.at ( 0,1 ), R.at ( 0,2 ),
               R.at ( 1,0 ), R.at ( 1,1 ), R.at ( 1,2 ),
               R.at ( 2,0 ), R.at ( 2,1 ), R.at ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at ( 0,0 ), t.at ( 1,0 ), t.at ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );

    /*路标顶点*/
    int index = 1;
    for ( const Point3f p:points_3d )
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }

    /*相机参数*/
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at ( 0,0 ), Eigen::Vector2d ( K.at ( 0,2 ), K.at ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    /*边*/
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast ( 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++;
    }

    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    cout</**
 * \brief SE3 Vertex parameterized internally with a transformation matrix
 and externally with its exponential map
 */
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VertexSE3Expmap();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

路标点顶点如下:

 class G2O_TYPES_SBA_API VertexSBAPointXYZ : public BaseVertex<3, 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::Map v(update);
            _estimate += v;
        }
};

在边的参数中,2表示测量值是二维的(图像坐标),Vector2D表示测量值的类型,两个顶点分别是路标点和位姿。

具体的类封装如下:

class G2O_TYPES_SBA_API EdgeProjectXYZ2UV : public  BaseBinaryEdge<2, Vector2D, VertexSBAPointXYZ, VertexSE3Expmap>
{
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        EdgeProjectXYZ2UV();    /*构造函数*/
        void computeError()    /*误差计算函数*/  
        {
            const VertexSE3Expmap* v1 = static_cast(_vertices[1]);    /*相机位姿*/ 
            const VertexSBAPointXYZ* v2 = static_cast(_vertices[0]);    /*顶点*/ 
            const CameraParameters * cam = static_cast(parameter(0));    /*相机参数*/ 
            /*误差计算,测量值减去估计值,也就是重投影误差obs-cam*/ 
            /*估计值计算方法是T*p,得到相机坐标系下坐标,然后计算像素坐标*/ 
            Vector2D obs(_measurement);
            _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
        }
        virtual void linearizeOplus();    /*计算雅可比*/ 
        CameraParameters * _cam;     /*相机参数*/ 
        bool read(std::istream& is);
        bool write(std::ostream& os) const;
};

其中计算误差computeError()函数的过程是,将3D世界坐标通过位姿变换转换成相加坐标下的3D点,再使用内参转换成像素坐标,并与观测到的像素坐标进行计算获取误差;

linearizeOplus函数是在当前顶点的值下,该误差对优化变量的偏导数(雅可比)该函数位于types_six_dof_expmap.cpp文件中定义如下:

void EdgeProjectXYZ2UV::linearizeOplus() {
  VertexSE3Expmap * vj = static_cast(_vertices[1]);
  SE3Quat T(vj->estimate());
  VertexSBAPointXYZ* vi = static_cast(_vertices[0]);
  Vector3 xyz = vi->estimate();
  Vector3 xyz_trans = T.map(xyz);

  number_t x = xyz_trans[0];
  number_t y = xyz_trans[1];
  number_t z = xyz_trans[2];
  number_t z_2 = z*z;

  const CameraParameters * cam = static_cast(parameter(0));

  Matrix tmp;
  tmp(0,0) = cam->focal_length;
  tmp(0,1) = 0;
  tmp(0,2) = -x/z*cam->focal_length;

  tmp(1,0) = 0;
  tmp(1,1) = cam->focal_length;
  tmp(1,2) = -y/z*cam->focal_length;

  _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

  _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
  _jacobianOplusXj(0,2) = y/z *cam->focal_length;
  _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
  _jacobianOplusXj(0,4) = 0;
  _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

  _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
  _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
  _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
  _jacobianOplusXj(1,3) = 0;
  _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
  _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
}

代码中的雅可比与之前推导的相同,成员变量“_jacobianOplusXi”是误差到空间点的导数,“_jacobianOplusXj”是误差到相机位姿的导数。

 

6. g2o两图像间ICP方法位姿优化

源码地址:https://github.com/gaoxiang12/slambook/blob/master/ch7/pose_estimation_3d2d.cpp

代码的整个流程是,已知世界坐标系特征点的3D坐标(通过三角化得到),已知位姿变换后的对应特征点的3D坐标,并使用ICP法(【VSLAM】8——视觉里程计之ICP)计算出相应的R和t后,使用BA方法构建最小二乘问题并进行位姿的优化。

void bundleAdjustment (
    const vector< Point3f >& pts1,
    const vector< Point3f >& pts2,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverEigen(); // 线性方程求解器
    //Block* solver_ptr = new Block( linearSolver );      // 矩阵块求解器
    Block* solver_ptr = new Block( std::unique_ptr(linearSolver) );
    g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::unique_ptr(solver_ptr) );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm( solver );

    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    pose->setId(0);
    pose->setEstimate( g2o::SE3Quat(
        Eigen::Matrix3d::Identity(),
        Eigen::Vector3d( 0,0,0 )
    ) );
    optimizer.addVertex( pose );

    // edges
    int index = 1;
    vector edges;
    for ( size_t i=0; isetId( index );
        edge->setVertex( 0, dynamic_cast (pose) );
        edge->setMeasurement( Eigen::Vector3d( 
            pts1[i].x, pts1[i].y, pts1[i].z) );
        edge->setInformation( Eigen::Matrix3d::Identity()*1e4 );
        optimizer.addEdge(edge);
        index++;
        edges.push_back(edge);
    }

    optimizer.setVerbose( true );
    optimizer.initializeOptimization();
    optimizer.optimize(10);

    cout</**
 * \brief SE3 Vertex parameterized internally with a transformation matrix
 and externally with its exponential map
 */
class G2O_TYPES_SBA_API VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VertexSE3Expmap();

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  virtual void setToOriginImpl() {
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const number_t* update_)  {
    Eigen::Map update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

边为自定义的边,由于只有一个节点,所以边为一元边(继承BaseUnaryEdge)。在g2o/sba中没有提供3D-3D的边,所以可将边自定义:

class EdgeProjectXYZRGBDPoseOnly : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjectXYZRGBDPoseOnly( const Eigen::Vector3d& point ) : _point(point) {}

    virtual void computeError()
    {
        const g2o::VertexSE3Expmap* pose = static_cast ( _vertices[0] );
        // measurement is p, point is p'
        _error = _measurement - pose->estimate().map( _point );
    }
    
    virtual void linearizeOplus()
    {
        g2o::VertexSE3Expmap* pose = static_cast(_vertices[0]);
        g2o::SE3Quat T(pose->estimate());
        Eigen::Vector3d xyz_trans = T.map(_point);
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
        
        _jacobianOplusXi(0,0) = 0;
        _jacobianOplusXi(0,1) = -z;
        _jacobianOplusXi(0,2) = y;
        _jacobianOplusXi(0,3) = -1;
        _jacobianOplusXi(0,4) = 0;
        _jacobianOplusXi(0,5) = 0;
        
        _jacobianOplusXi(1,0) = z;
        _jacobianOplusXi(1,1) = 0;
        _jacobianOplusXi(1,2) = -x;
        _jacobianOplusXi(1,3) = 0;
        _jacobianOplusXi(1,4) = -1;
        _jacobianOplusXi(1,5) = 0;
        
        _jacobianOplusXi(2,0) = -y;
        _jacobianOplusXi(2,1) = x;
        _jacobianOplusXi(2,2) = 0;
        _jacobianOplusXi(2,3) = 0;
        _jacobianOplusXi(2,4) = 0;
        _jacobianOplusXi(2,5) = -1;
    }

    bool read ( istream& in ) {}
    bool write ( ostream& out ) const {}
protected:
    Eigen::Vector3d _point;
};

在计算误差函数中,将作为参数的相机坐标3D点进行位姿变换,获取相应的世界坐标,并与边中记录的世界坐标3D点做差得到误差值。

linearizeOplus函数列出了误差对优化变量的雅可比,如之前的雅可比所述。

另外,除了相机位姿之外,可也将空间点作为优化变量进行优化迭代,对优化问题添加更多的约束。

 

 

你可能感兴趣的:(VSLAM)