前言
g2o是比较流行的图优化库，在视觉slam中的应用非常广泛，关于如何利用g2o求解BA优化问题，在此作个笔记，以供后面的学习中方便查阅。
一、g2o进行非线性优化
以《视觉slam十四讲》中的求解最小二乘问题为例：
原函数：
二范数：
误差函数：
构建图模型：顶点为待优化变量，边为误差。图的模型大致为下图(有点丑。。。)。
因为我们需要求的量只有一个，即：vector3d(a,b,c),因此，在这个图中，顶点只有一个，我们通过随机生成n组 x,y，他们之间的误差构成了许许多多的边。构建出了图之后，下面以原书程序进行讲解整个优化的编程实现过程：
// 构建图优化，先设定g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType; // 定义图的类型，顶点为三维，边为一维
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 梯度下降方法，可以从GN, LM, DogLeg 中选
auto solver = new g2o::OptimizationAlgorithmGaussNewton(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// 往图中增加顶点
CurveFittingVertex *v = new CurveFittingVertex();
v->setEstimate(Eigen::Vector3d(ae, be, ce));
v->setId(0);
optimizer.addVertex(v);
// 往图中增加边
for (int i = 0; i < N; i++) {
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
edge->setId(i);
edge->setVertex(0, v); // 设置连接的顶点
edge->setMeasurement(y_data[i]); // 观测数值
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge);
}
// 执行优化
cout << "start optimization" << endl;
optimizer.initializeOptimization();
optimizer.optimize(10);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
// 输出优化值
Eigen::Vector3d abc_estimate = v->estimate();
cout << "estimated model: " << abc_estimate.transpose() << endl;
其实整个图优化的过程大致都是这个框架，下面主要说下注意部分：
v->setId(0);
这句程序是对顶点进行编号，里面的0你可以写成任意的正整数，但是后面设置edge连接顶点时，必须要和这个一致。
// 往图中增加边
for (int i = 0; i < N; i++) {
CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
edge->setId(i);
edge->setVertex(0, v); // 设置连接的顶点
edge->setMeasurement(y_data[i]); // 观测数值
edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
optimizer.addEdge(edge);
}
该部分程序需要注意的是edge->setVertex(0, v); 其中的0为该边连接的第一个顶点，即前面编号为0的顶点v，因为是一元边，因此只需这一句。根据函数公式：
可知，y为观测值，故有edge->setMeasurement(y_data[i])。图构建出之后，即可进行迭代求解。
二、g2o进行前端位姿优化
在《视觉slam十四讲》中，前端的位姿优化主要讲了3D-3D的ICP、3D-2D的PNP位姿求解，本文主要讲解ICP的g2o求解。其实，这和前面的非线性优化很相似，两帧图像之间匹配出的3D点的差值构成了该图的一元边，顶点即为两帧之间的运动估计T，也就是相机的位姿，两帧之间优化出一个相机位姿T。
二范数：
图模型：
下面为g2o构建图过程以及图优化的代码编写，同样以十四讲书中的程序为例。
// 设定g2o，构建图模型
typedef g2o::BlockSolverX BlockSolverType;
typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型
// 选择优化方法
auto solver = new g2o::OptimizationAlgorithmLevenberg(
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
// vertex
VertexPose *pose = new VertexPose(); // camera pose
//两帧优化出一个最优pose 一个edge连接一个vertex
pose->setId(0);
pose->setEstimate(Sophus::SE3d()); //李群T
optimizer.addVertex(pose);
边的部分
//边的值即为两帧之间匹配点的3D坐标之差
for (size_t i = 0; i < pts1.size(); i++) {
EdgeProjectXYZRGBDPoseOnly *edge = new EdgeProjectXYZRGBDPoseOnly(
Eigen::Vector3d(pts2[i].x, pts2[i].y, pts2[i].z));
//因为是一元边，故只设置连接的第一个顶点
edge->setVertex(0, pose);
// P1 = T12P2，故观测值为pts1
edge->setMeasurement(Eigen::Vector3d(
pts1[i].x, pts1[i].y, pts1[i].z));
edge->setInformation(Eigen::Matrix3d::Identity());
optimizer.addEdge(edge);
}
对构建的图进行优化
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "optimization costs time: " << time_used.count() << " seconds." << endl;
cout << endl << "after optimization:" << endl;
cout << "T=\n" << pose->estimate().matrix() << endl;
需要注意的是，因为设置的观测值为p1，所以优化的pose为p2到p1的坐标变换。 如果想要求得一个图像序列中，两两帧之间的T，那么只需要循环的调用
void bundleAdjustment(const vector<Point3f> &pts1,const vector<Point3f> &pts2, Mat &R, Mat &t)
这个函数，传入的参数为两帧之间匹配的3D点对即可。
三、g2o进行后端位姿优化
高翔博士的一起做RGBD-SLAM中有一个是利用g2o对vo进行了一个后端优化的讲解，我们也以此为例。
在前端的vo求解中，利用PNP或者ICP求出了两帧之间的相机运动(局部最优)，连接起来即为视觉里程计，但这个过程中存在着误差累积，且一旦出现了错误匹配，里程计的误差将会逐渐增加，将整体的位姿进行优化，有助于误差的消除，达到全局的最优。
原函数：
二范数：
xi∗表示xi的估计值。
图模型：
与前面图模型不同的是，后端优化是将所有的pose相连，以第一个pose为初始值，整体进行迭代优化，edge为二元边。
图优化程序如下：
顶点部分：初始化第一个顶点
/*******************************
// 新增:有关g2o的初始化
*******************************/
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,6>> SlamBlockSolver; //图类型(顶点6维,边6维)
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; //线性求解器类型
//选择优化方法
auto solver = new g2o::OptimizationAlgorithmLevenberg(g2o::make_unique<SlamBlockSolver>(g2o::make_unique<SlamLinearSolver>()));
g2o::SparseOptimizer globalOptimizer;
globalOptimizer.setAlgorithm( solver );
// 不要输出调试信息
globalOptimizer.setVerbose( false );
// 向globalOptimizer增加第一个顶点
g2o::VertexSE3* v = new g2o::VertexSE3();
//第一个顶点为初始值，编号为初始帧currIndex
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵
v->setFixed( true ); //第一个顶点固定，不用优化
globalOptimizer.addVertex( v );
边部分：即为PNP估计出的T，也即相机位姿。
初始化第二个顶点，为当前帧的编号
这部分代码是在一个for循环中，主要就是向图中逐个的加入边和顶点。
// 向g2o中增加这个顶点与上一帧联系的边
// 顶点部分
// 顶点只需设定id即可
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() );
globalOptimizer.addVertex(v);
// 边部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 连接此边的两个顶点id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
// 信息矩阵
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计
// 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立
// 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以将角度设大一些，表示对角度的估计更加准确
edge->setInformation( information );
// 边的估计即是pnp求解之结果
edge->setMeasurement( T );
// 将此边加入图中
globalOptimizer.addEdge(edge);
因为是后端的整体优化，所以需要将全部的相机位姿通过二元边进行连接起来，然后对构建的图进行优化。
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
该边连接的第一个顶点为上一帧的T，第二个为当前帧的T。
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定优化步数
globalOptimizer.clear();
总结
值得注意的是，在利用g2o进行图优化时，我们优化的公式为
但我们在进行点云拼接时，会想求这样的公式
中的T，所以需要获得pose->estimate().inverse()进行点云拼接。
参考资料：
1313
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
