graph slam tutorial ：从推导到应用3（g2o+ceres实现）

Posted 2022-12-09 chengwei0019

目录

图的数据格式  

读取数据

使用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.652690

killian-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<double, 3, 3> 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/