我們不生產代碼，我們只是代碼的搬運工。

圖的數據格式

圖優化前端處理以後產生的頂點和邊的數據格式，這是寫程序時特別關注的，也是衆多優化包如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);
	}

生成結果如下：