graph slam tutorial ：从推导到应用3（g2o+ceres实现）
目录
图的数据格式
读取数据
使用g2o中slam2d数据格式的优化
优化结果
定义顶点和边
我们不生产代码，我们只是代码的搬运工。
图的数据格式
图优化前端处理以后产生的顶点和边的数据格式，这是写程序时特别关注的，也是众多优化包如g2o的数据格式。
matlab程序及数据戳这里
killian-v.dat 数据为顶点vertex2: id，pose.x，pose.y，pose.theta 其中id表示位姿的序号，后面三个是位姿参数（如下所示）
VERTEX2 0 1.008240 -0.016781 0.005957
VERTEX2 1 2.090063 0.008002 0.015650
VERTEX2 2 3.117849 -0.027274 0.023100
VERTEX2 3 4.198081 0.087164 0.035227
VERTEX2 4 5.279355 0.111386 0.045086
VERTEX2 5 6.263466 0.126602 -0.005163
VERTEX2 6 7.283441 0.076036 -0.015700
VERTEX2 7 8.357930 0.058120 -0.015358
VERTEX2 8 9.390035 -0.012710 0.017520
VERTEX2 9 10.408369 -0.056070 -0.208266
VERTEX2 10 11.431652 -0.393503 -0.373306
VERTEX2 11 12.421098 -0.716382 -0.409338
VERTEX2 12 12.938025 -1.136982 -0.920065
VERTEX2 13 13.179887 -1.862584 -1.469585
VERTEX2 14 13.122046 -2.788254 -1.652690killian-e.dat 数据为 边EDGE2: idFrom idTo mean.x mean.y mean.theta inf.xx inf.xy inf.xt inf.yy inf.yt inf.tt 其中idfrom,idTo表示连接边的两个位姿顶点序号，mean.xytheta表示测量的位姿变换矩阵。inf表示边的信息矩阵即权重。
EDGE2 1 0 -1.082078 -0.007851 -0.009693 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 2 1 -1.026697 0.059006 -0.007450 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 3 2 -1.083593 -0.076320 -0.012128 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 4 3 -1.081267 0.024536 -0.009858 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 5 4 -0.984019 -0.020296 0.050248 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 6 5 -1.020643 0.034546 0.010538 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 7 6 -1.074638 0.001413 -0.000343 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 8 7 -1.030705 0.088901 -0.032878 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 9 8 -1.005294 -0.168131 0.225786 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 10 9 -1.075867 -0.058994 0.165040 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 11 10 -1.036209 -0.097597 0.036032 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000
EDGE2 12 11 -0.647785 -0.156502 0.510727 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000读取数据
读取代码很简单，直接上代码
if (!getline(fin, temp_line))
break;
sscanf_s(temp_line.c_str(), "VERTEX2 %d %lf %lf %lf",
&vertexID, &vertexX, &vertexY, &vertexTheta);
sVertex vertex;
vertex.id = vertexID;
vertex.vx = vertexX, vertex.vy = vertexY, vertex.vt = vertexTheta;使用g2o中slam2d数据格式的优化
顶点g2o::VertexSE2
for (int i = 0; i < vecVertex.size(); i++)
{
g2o::VertexSE2* vertex = new g2o::VertexSE2;
vertex->setId(i);
if (i == 0)
vertex->setFixed(true);
vertex->setEstimate(g2o::SE2(vecVertex[i].vx, vecVertex[i].vy, vecVertex[i].vt));
optimizer.addVertex(vertex);
}边g2o::EdgeSE2
for (int i = 0; i < vecEdge.size(); i++)
{
sEdge e = vecEdge[i];
g2o::EdgeSE2* edge = new g2o::EdgeSE2;
edge->vertices()[0] = optimizer.vertex(e.from);
edge->vertices()[1] = optimizer.vertex(e.to);
edge->setMeasurement(g2o::SE2(e.mx, e.my, e.mt));
Eigen::Matrix information = Eigen::Matrix< double, 3, 3 >::Identity();
information(0, 0) = e.infm1; information(1, 0) = information(0, 1) = e.infm2;
information(1, 1) = e.infm3; information(2, 2) = e.infm4;
information(0, 2) = information(2, 0) = e.infm5;
information(2, 1) = information(1, 2) = e.infm6;
edge->setInformation(information);
optimizer.addEdge(edge);
} 优化结果
g2o优化前
g2o优化后
完整代码戳这里
代码里默认使用稠密的增量方程，稀疏的增量方程也已经给出。稠密的增量方程运行太慢，建议使用稀疏的增量方程。
这里只是做了一个简单的尝试，也有一些取巧；后续准备自己定义顶点和边，写一个完整的优化。敬请期待。。。
定义顶点和边
尝试自定义顶点和边，但是无法直接调用save("*.g2o")保存可供g2o_viewer观看的数据了，还不知道为什么。做了个取巧的办法，保存优化前后每个顶点的坐标值，用matlab显示效果如下。
update2019-11-07：g2o格式数据无法直接保存，（猜测）原因为自定义顶点和边，save函数并不能识别；可以自己仿照g2o内部格式，直接写到文本里；也可以在顶点和边里实现read和write函数，然后直接write。（仅为猜测，未去验证）
由于还没有理清楚，就先不写出来了，后面理清楚了，会更新此博客。
利用ceres优化
代码如下：
for (auto vec : vecEdge)
{
if (vec.from >= vecVertex.size() || vec.from < 0)
std::cout << "Pose with ID: " << vec.from << " not found." << std::endl;
if (vec.to >= vecVertex.size() || vec.to < 0)
std::cout << "Pose with ID: " << vec.to << " not found." << std::endl;
Eigen::Matrix3d sqrt_information = Eigen::Matrix3d::Identity();
sqrt_information(0, 0) = vec.infm1; sqrt_information(1, 0) = sqrt_information(0, 1) = vec.infm2;
sqrt_information(1, 1) = vec.infm3; sqrt_information(2, 2) = vec.infm4;
sqrt_information(0, 2) = sqrt_information(2, 0) = vec.infm5;
sqrt_information(2, 1) = sqrt_information(1, 2) = vec.infm6;
ceres::CostFunction* cost_function = ceres::examples::PoseGraph2dErrorTerm::Create(
vec.mx, vec.my, vec.mt, sqrt_information);
problem.AddResidualBlock(
cost_function, loss_function, &vecVertex[vec.from].vx,
&vecVertex[vec.from].vy, &vecVertex[vec.from].vt,
&vecVertex[vec.to].vx, &vecVertex[vec.to].vy,
&vecVertex[vec.to].vt);
problem.SetParameterization(&vecVertex[vec.from].vt,
angle_local_parameterization);
problem.SetParameterization(&vecVertex[vec.to].vt,
angle_local_parameterization);
}生成结果如下：
对比g2o和ceres优化结果：
有些细微的差异，应该是跟参数配置有关。
参考：
https://blog.csdn.net/heyijia0327/article/details/47428553
https://github.com/versatran01/graphslam
http://www.dis.uniroma1.it/~grisetti/teaching/lectures-ls-slam-master/web/