slam14讲后端优化代码注释（二）（第二版ch10）
代码目录
- 一、pose_graph_g2o_lie_algebra.cpp
- 二、pose_graph_g2o_SE3.cpp
一、pose_graph_g2o_lie_algebra.cpp
#include > BalBlockSolver。
3、而在位姿图（pose graph）里面，顶点是位姿，边也是位姿，
变成了typedef g2o::BlockSolver> Block。
先说结论，P,L代表你定义的边所对应的两个顶点的维度，block里面只包含顶点维度，至于边的维度是隐含在边的定义里面的。
所以你看2里面，一条边两头分别是cam(9维度)和point(3维)，这条边本身只有2维，代表像素坐标，error[0,1] = camproject(cam, point)[0,1] - observation[0,1].
再看3，边的两头分别是SE3(平移加旋转6维)，所以block<6,6>.
最后1的特殊性在于它是一条unary edge，所以只有一个头，顶点是abc[3]，所以error = abc[0] *x^2 + abc[1] * x + abc[2] - observation. 所以一般设置block的时候第二个就是1了
*/
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 6>> BlockSolverType; //块求解器
typedef g2o::LinearSolverEigen<BlockSolverType::PoseMatrixType> LinearSolverType; //线性求解器
auto solver = new g2o::OptimizationAlgorithmLevenberg( //设置总求解器（块求解器（线性求解器）） 求解方式L-M
g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
g2o::SparseOptimizer optimizer; // 图模型
optimizer.setAlgorithm(solver); // 设置求解器
optimizer.setVerbose(true); // 打开调试输出
int vertexCnt = 0, edgeCnt = 0; // 顶点和边的数量
/*
.g2o文件的数据格式
1.顶点 ---相机坐标
VERTEX_SE3:QUAT
点的索引值 平移 旋转
point id t.x t.y t.z qx qy qz qw
2.边
EDGE_SE3:QUAT
idFrom idTo t.x t.y t.z qx qy qz qw 信息矩阵的右上角
*/
vector<VertexSE3LieAlgebra *> vectices;
vector<EdgeSE3LieAlgebra *> edges;
while (!fin.eof()) {
string name;
fin >> name;
if (name == "VERTEX_SE3:QUAT") {
// 顶点
VertexSE3LieAlgebra *v = new VertexSE3LieAlgebra();
int index = 0;
fin >> index;
v->setId(index);
v->read(fin); //在自定义顶点类中VertexSE3LieAlgebra重写read（）函数并调用setEstimate（）实现顶点的初始化
optimizer.addVertex(v); //增加顶点
vertexCnt++;
vectices.push_back(v);
if (index == 0)
//this node should be considered fixed during the optimization
//初始位姿，在整个优化过程中不变（固定）
v->setFixed(true);
} else if (name == "EDGE_SE3:QUAT") {
// SE3-SE3 边
EdgeSE3LieAlgebra *e = new EdgeSE3LieAlgebra();
int idx1, idx2; // 关联的两个顶点
fin >> idx1 >> idx2;
e->setId(edgeCnt++);
//文件中数据确定了每条边关联的顶点（两个位姿点）
e->setVertex(0, optimizer.vertices()[idx1]);
e->setVertex(1, optimizer.vertices()[idx2]);
e->read(fin); //读取edg的位置（旋转平移） 和信息矩阵information() ， 用setMeasurement（）设置测量值
optimizer.addEdge(e); //增加边
edges.push_back(e);
}
if (!fin.good()) break;
}
cout << "read total " << vertexCnt << " vertices, " << edgeCnt << " edges." << endl;
cout << "optimizing ..." << endl;
optimizer.initializeOptimization(); //初始化
optimizer.optimize(30); //设置迭代次数
cout << "saving optimization results ..." << endl;
// 因为用了自定义顶点且没有向g2o注册，这里保存自己来实现
// 伪装成 SE3 顶点和边，让 g2o_viewer 可以认出
ofstream fout("result_lie.g2o"); //输出文件流
for (VertexSE3LieAlgebra *v:vectices) {
fout << "VERTEX_SE3:QUAT ";
v->write(fout);
}
for (EdgeSE3LieAlgebra *e:edges) {
fout << "EDGE_SE3:QUAT ";
e->write(fout);
}
fout.close(); //关闭文件
return 0;
}
二、pose_graph_g2o_SE3.cpp
#include