C++实现ICP点云匹配

 代码流程说明：

1. 读取源点云SC；

2. 构造目标点云TC；

3. 初始化旋转矩阵和平移矩阵；

4. 根据旋转矩阵和平移矩阵，变换目标点云，得到新的目标点云TC_1；

5. 查找近邻点对，得到重排序的源点云SC_1；

6. 分别去中心得到SC_2,TC_2：

7. 对应点对矩阵和，SVD分解得到新的旋转矩阵和平移矩阵；

重复4-7步骤，直到对应点对位置差小于预设阈值；

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/eigen>

using namespace cv;
using namespace std;

void calNearestPointPairs(Eigen::Matrix3f R, Eigen::Vector3f t, vector<Point3f>& pts1, vector<Point3f>& pts2, vector<Point3f>& outPoints, vector<float> &error)
{
	outPoints.clear();
	error.clear();

	int num = pts1.size();
	//对目标点云进行变换

	for (int i = 0; i < num; i++)
	{
		float x = Eigen::Vector3f(R(0, 0), R(0, 1), R(0, 2)).transpose() * Eigen::Vector3f(pts2[i].x, pts2[i].y, pts2[i].z) + t[0];