《视觉SLAM十四讲精品总结》8：VO—— 3D-3D：ICP+SVD+BA

一、ICP

ICP（Iterative Closest Point）迭代最近点，指代匹配好的两组点云间运动估计问题。

ICP算法是对于X中的每一个点用当前的R和 t在Y中找最近的点（比如用欧式距离），然后这两个点就成了一对了，每个点都有了对应的映射点，用每一对的坐标列一个方程，就得到一系列的方程。

重复迭代运行上述过程，直到均方差误差小于某个阀值。 

二、RANSAC

 （RANdom SAmple Consensus随机抽样一致），它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。这个算法要算出的就是蓝色部分那个模型的参数

RANSAC算法的输入是一组观测数据，一个可以解释或者适应于观测数据的参数化模型，一些可信的参数。

模型对应的是空间中一个点云数据到另外一个点云数据的旋转以及平移。

第一步随机得到的是一个点云中的点对作 ，利用其不变特征（两点距离，两点法向量夹角）作为哈希表的索引值搜索另一个点云中的一对对应点对，然后计算得到旋转及平移的参数值。

然后适用变换，找到其他局内点，并在找到局内点之后重新计算旋转及平移为下一个状态。

然后迭代上述过程，找到最终的位置

三、代码解析

1、主框架 

int main(int argc, char** argv)
{

	Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
	Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
	Mat depth1 = imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
	Mat depth2 = imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);       // 深度图为16位无符号数，单通道图像
	//1. 找关键点和匹配关系
	vector<KeyPoint> keypoints_1, keypoints_2;
	vector<DMatch> matches;
	find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
	cout << "一共找到了" << matches.size() << "组匹配点" &