V-SLAM 回环检测与后端优化重读

研究背景： 后端优化原理大多人都比较清楚，但是实现起来不是个容易的事情，而回环检测部分在自己测试之后发现，这部分在vSLAM中也是至关重要的，本文的给出了在没有好的回环下得到的地图样例。

1. 几个坏的回环结果

2. 问题描述

• 误差积累： 相机转过去的过程正常运行，但转回来时会出现明显偏差;

• 点云重建效率：在线点云更新速度较慢, 不实时显示速度影响不大.

3. VO－视觉里程计的问题

• 帧----帧，匹配获取相机位姿

相邻帧匹配计算相机位姿，容易造成误差累计。

• 帧----旧地图，匹配获取相机位姿

理论上较为稳定，因为可以跟之前多个时刻的数据进行对比更新。

• g2o优化相机位姿

这里给出半闲居士博客的一张图供参考（意思是代码实现过程中的函数之间的调用关系）：

并给出自己对图优化理解一点小笔记:

• g2o程序编写（核心）

		//  初始化
		 	g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >* linearSolver
		 	 = new g2o::LinearSolverEigen< g2o::BlockSolver_6_3::PoseMatrixType >();
			g2o::BlockSolver_6_3* blockSolver = new g2o::BlockSolver_6_3(linearSolver);
			g2o::OptimizationAlgorithmLevenberg* solver
			= new g2o::OptimizationAlgorithmLevenberg( blockSolver );	
			optimizer.setAlgorithm( solver );
			optimizer.setVerbose( false );
	
		// 初始顶点	
			g2o::VertexSE3* v = new g2o::VertexSE3();
		    v->setId( cutIndex );
		    v->setEstimate( Eigen::Isometry3d::Identity() ); 
		    v->setFixed( true ); 
		    optimizer.addVertex( v )
		// 添加新的节点（顶点）
			g2o::VertexSE3* vNext = new g2o::VertexSE3();
			vNext->setId(currFrame.frameID);
			vNext->setEstimate( Eigen::Isometry3d::Identity());
			optimizer.addVertex(vNext);
	
		// 添加边的数据
			g2o::EdgeSE3* edgeNew = new g2o::EdgeSE3();
			edgeNew->setVertex(0, optimizer.vertex(pousFrame.frameID));

			edgeNew->setVertex(1, optimizer.vertex(currFrame.frameID));
	
			edgeNew->setRobustKernel( new g2o::RobustKernelHuber() );
			//edgeNew->setInformation( Eigen::Matrix2d::Identity());  //the size is worthy to research
		// 信息矩阵

		// 两个节点的转换矩阵
			Eigen::Isometry3d T = cvMat2Eigen( m_result.rotaMatrix, m_result.tranMatrix );	
			edgeNew->setMeasurement(T);
			optimizer.addEdge(edgeNew);
		// 开始优化
			optimizer.initializeOptimization();
			optimizer.optimize(10);

4. 回环检测

前端误差累计，使得长期估计的结果不可靠，无法构建全局一致的地图；

回环检测的目的是检测出相机是否经过同一个地方, 它是保障地图能够在长时间下正确运行的关键技术。

• 什么时候地图应该检测是否回环？

  1.随机检测：随机抽取部分历史数据进行回环检测，回环几率降低，效率较低；

  2.基于里程计的几何关系(Odometry based)：累计误差较大时无法工作，且无法正确发现“运动之前某个位置附近”的事实。

  3.基于外观的相似性(Appearance based)：与前端、后端都无关，仅根据两幅图像的相似性确定是否进行回环检测，这种方法摆脱了累计误差，被广泛应用。

• 如何有效、合理、准确地评估图像之间的相似性？

	"感知偏差"　　"感知变异"
	"准确率+ 召回率" 来评判估计图像相似性计算方法的 "好  -- 不好"

• 词袋模型 (bag-of-words) —>DBoW3

词袋：也就是收藏了很多不同种特征的词典，当我们需要用到词袋模型时，实际上就是将图像上的特征与词典中收藏的特征进行匹配得分，最后的得分就是评价图像是否相似的标准，也是判断是否可以回环的标准。

1.字典如何使用?
/*	
	1>　图像中的特征与字典中`words`对应(但不是一一对应)，将图像量化成一个包含各种特征的向量.
	2>　每个特征在字典中占据不同的权重；
	3>　与字典中匹配的特征，只强调是否出现，而无关顺序和位置，只是一个words的集合。
*/

2.如何创建字典？
/*
	1> words与一个单独的特征点不同，words是某一类特征的组合；
	2> 假如我要创建一个具有k个单词的字典，每个单词就可以看做局部相邻特征点的一个集合，简单的聚类(k-means)方法就可以实现上述功能。
*************************************************************************************************************
	3> 一个特征点对应一个单词，一张图像有N个特征点，就会对应N个单词，那么实际上就获取了这张图像重要信息在字典中的分布。
	4> 前者是对单词"一视同仁"， 然而实际中不同单词重要性不一;
			因此需要对单词的区分性和重要性加以评估，赋予单词不同的权重将更有意义。("TF-IDF")
*/

• 特征–words相似性评分

	1> 当相似性得分区别不大时，可扩展字典的规模，重新检测；
	2> 为使算法适应更多的环境（有很多相似之处的环境， 以及外观有很大差别的环境），给出一个"先验相似度", 
									将每一个在上一步得到的相似性得分除以这个先验相似度，就可以得到归一化后的分值。
	 "那么，当当前帧与之前某一个关键帧的分值大于当前帧与上一个关键帧的3倍时， 就认为可能存在回环 !"

• 检测到回环之后的验证

	1>  "时间上的一致性检测：" 单次回环不足以构成良好的约束，在一段时间检测到的回环，才认为是正确的回环。
	2>  "空间上的一致性检测：" 对回环检测到的两个帧进行特征匹配，估计相机运动，然后与之前的位姿图进行对比，检测是否有很大出入。