graph slam tutorial : 從推導到應用1（g2o實現）

目錄

例子1：

例子2：

---

例子1：

 爲了更好的理解這個過程，將用一個很好的例子作說明。如下圖所示，假設一個機器人初始起點在0處，然後機器人向前移動，通過編碼器測得它向前移動了1m，到達第二個地點。接着，又向後返回，編碼器測得它向後移動了0.8米。但是，通過閉環檢測，發現它回到了原始起點。可以看出，編碼器誤差導致計算的位姿和觀測到有差異，那機器人這幾個狀態中的位姿到底是怎麼樣的才最好的滿足這些條件呢？

定義頂點和邊：

class MyVertex : public g2o::BaseVertex<1, double>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
		virtual void setToOriginImpl()
	{
		_estimate = 0.0;
	}
	virtual void oplusImpl(const double* update)
	{
		_estimate += (*update);
	}

	virtual bool read(istream & in) { return 0; }
	virtual bool write(ostream & out) const { return 0; }
};

class MyEdge : public g2o::BaseBinaryEdge<1, double, MyVertex, MyVertex>
{
public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
		void computeError()
	{
		const MyVertex * v1 = static_cast<const MyVertex*>(_vertices[0]);
		const MyVertex * v2 = static_cast<const MyVertex*>(_vertices[1]);
		_error(0, 0) = _measurement - (v2->estimate() - v1->estimate());
	}
	virtual bool read(istream & in) { return 0; }
	virtual bool write(ostream & out) const { return 0; }
};

主函數：

int main()
{
	//	從x0到x1，編碼器測量移動了1m，從x1到x2，編碼器測量移動了-0.8，觀測發現x2和x0重合。
	double edge1 = 1, edge2 = -0.8, edge3 = 0;

	typedef g2o::BlockSolver<g2o::BlockSolverTraits<1, 1>> Block;
	// std::unique_ptr<Block::LinearSolverType> linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>());
	// std::unique_ptr<Block> solver_ptr ( new Block(std::move(linearSolver)));

	g2o::SparseOptimizer optimizer;
	Block::LinearSolverType * linearSolver;
	linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	Block * solver_ptr = new Block(std::unique_ptr< Block::LinearSolverType>(linearSolver));

	g2o::OptimizationAlgorithmLevenberg * solver
		= new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr < Block>(solver_ptr));
	optimizer.setAlgorithm(solver);
	optimizer.setVerbose(true);

	MyVertex *v1 = new MyVertex;
	v1->setId(0);
	v1->setEstimate(0);
	v1->setFixed(true);
	optimizer.addVertex(v1);

	MyVertex *v2 = new MyVertex;
	v2->setId(1);
	v2->setEstimate(1);
	v2->setFixed(false);
	optimizer.addVertex(v2);

	MyVertex *v3 = new MyVertex;
	v3->setId(2);
	v3->setEstimate(0);
	v3->setFixed(false);
	optimizer.addVertex(v3);

	MyEdge * e1 = new MyEdge;
	MyEdge * e2 = new MyEdge;
	MyEdge * e3 = new MyEdge;

	e1->setId(1);
	e1->setVertex(0, v1);
	e1->setVertex(1, v2);
	e1->setMeasurement(1);
	e1->setInformation(Eigen::Matrix<double, 1, 1>::Identity());

	e2->setId(2);
	e2->setVertex(0, v2);
	e2->setVertex(1, v3);
	e2->setMeasurement(-0.8);
	e2->setInformation(Eigen::Matrix<double, 1, 1>::Identity());

	e3->setId(3);
	e3->setVertex(0, v3);
	e3->setVertex(1, v1);
	e3->setMeasurement(0);
	e3->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
	optimizer.addEdge(e1);
	optimizer.addEdge(e2);
	optimizer.addEdge(e3);

	cout << "start optimization" << endl;
	optimizer.initializeOptimization();
	optimizer.optimize(10);
	cout << v1->estimate() << ' ' << v2->estimate() << ' ' << v3->estimate() << endl;

	system("pause");
	return 0;
}

運行結果：

---

例子2：

頂點和邊的定義一樣，主函數如下：

int main()
{
	//	從x0處觀測到前方2m存在路標，移動到x1，編碼器測量移動了1m，此時觀測到路標在其前方0.8m。
	typedef g2o::BlockSolver<g2o::BlockSolverTraits<1, 1>> Block;
	// std::unique_ptr<Block::LinearSolverType> linearSolver( new g2o::LinearSolverDense<Block::PoseMatrixType>());
	// std::unique_ptr<Block> solver_ptr ( new Block(std::move(linearSolver)));

	g2o::SparseOptimizer optimizer;
	Block::LinearSolverType * linearSolver;
	linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
	Block * solver_ptr = new Block(std::unique_ptr< Block::LinearSolverType>(linearSolver));

	g2o::OptimizationAlgorithmLevenberg * solver
		= new g2o::OptimizationAlgorithmLevenberg(std::unique_ptr < Block>(solver_ptr));
	optimizer.setAlgorithm(solver);
	optimizer.setVerbose(true);

	MyVertex *v0 = new MyVertex;
	v0->setId(0);
	v0->setEstimate(0);
	v0->setFixed(true);
	optimizer.addVertex(v0);

	MyVertex *v1 = new MyVertex;
	v1->setId(1);
	v1->setEstimate(1);
	v1->setFixed(false);
	optimizer.addVertex(v1);

	MyVertex *lank = new MyVertex;
	lank->setId(2);
	lank->setEstimate(2);
	lank->setFixed(false);
	optimizer.addVertex(lank);

	MyEdge * e1 = new MyEdge;
	e1->setId(0);
	e1->setVertex(0, v0);
	e1->setVertex(1, lank);
	e1->setMeasurement(2);
	e1->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
	optimizer.addEdge(e1);

	MyEdge * e2 = new MyEdge;
	e2->setId(1);
	e2->setVertex(0, v1);
	e2->setVertex(1, lank);
	e2->setMeasurement(0.8);
	e2->setInformation(Eigen::Matrix<double, 1, 1>::Identity());
	optimizer.addEdge(e2);

	MyEdge * e3 = new MyEdge;
	e3->setId(1);
	e3->setVertex(0, v0);
	e3->setVertex(1, v1);
	e3->setMeasurement(1);
	e3->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);// 這裏的10爲傳感器權重
	optimizer.addEdge(e3);

	cout << "start optimization" << endl;
	optimizer.initializeOptimization();
	optimizer.optimize(10);
	cout << v0->estimate() << ' ' << v1->estimate() << ' ' << lank->estimate() << endl;

	system("pause");
	return 0;
}

運行結果：

《end here》

下一篇，寫一個基於g2o的完整slam後端

---

參考：

https://blog.csdn.net/heyijia0327/article/details/47686523

https://blog.csdn.net/wphkadn/article/details/90317006