点到面的ICP算法

参考：
点云配准算法ICP及其各种变体，值得一读！
基于高斯牛顿的点-点，点-线，点到面ICP过程推导
点云配准: 刚性ICP中 Point-to-Point 和 Point-to-Plane 公式推导 ==> 帮助你代码实现

使用点到平面（point-plane）误差度量的迭代最近点 (ICP) 算法已被证明比使用点到点（point-point）误差度量的算法收敛得更快。在 ICP 算法的每次迭代中，产生最小点到平面误差的相对位姿变化通常使用标准的非线性最小二乘法来解决。例如 Levenberg-Marquardt 方法。当使用点到平面误差度量时，最小化的对象是每个源点与其对应目标点的切平面之间的平方距离之和。

最小化损失函数可以写为：

其中是源点，是对应的目的点，是处的单位法线向量。这种方法的优缺点也很明显：首先，点到平面成本函数允许平坦区域相互滑动；点到平面通常比点到点收敛得更快；迭代次数更少 ；点到平面在每次迭代中速度较慢，并且需要表面法线。该式没有解析解，我们只能使用非线性最小二乘的方法求解，为了加快求解，有研究者发现当两个输入表面之间的相对方向较小时，可以使用近似的方法来有效地求解的线性最小二乘法近似非线性优化问题。
难以优化的原因是R 太复杂，无法优化。在每次迭代中，我们假设旋转的角度很小，即

那么旋转矩阵可以近似表示为：

重写损失函数为：

其中：

这样非线性的损失函数近似为线性，每一步迭代的最优解x为：

多次迭代后直到算法收敛。

#include <iostream>
#include <ctime>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <pcl/search/kdtree.h> 
#include <pcl/correspondence.h>
#include <pcl/features/normal_3d.h>
//#include <pcl/visualization/pcl_visualizer.h>


int main(int argc, char** argv)
{
    
    
	pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZLNormal>::Ptr source_cloud_mid(new pcl::PointCloud<pcl::PointXYZLNormal>);
	pcl::PointCloud<pcl::PointXYZLNormal>::Ptr target_cloud_mid(new pcl::PointCloud<pcl::PointXYZLNormal>);
	pcl::PointCloud<pcl::PointXYZLNormal>::Ptr target_cloud_copy(new pcl::PointCloud<pcl::PointXYZLNormal>);

	//加载点云
	pcl::io::loadPCDFile("bunny1.pcd", *source_cloud);
	pcl::io::loadPCDFile("bunny3.pcd", *target_cloud);

	//计算法向
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_source;
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne_target;
	pcl::PointCloud<pcl::Normal>::Ptr source_normals(new pcl::PointCloud<pcl::Normal>);
	pcl::PointCloud<pcl::Normal>::Ptr target_normals(new pcl::PointCloud<pcl::Normal>);

	ne_source.setInputCloud(source_cloud);
	ne_source.setKSearch(10);
	ne_source.compute(*source_normals);
	pcl::concatenateFields(*source_cloud, *source_normals, *source_cloud_mid);

	ne_target.setInputCloud(target_cloud);
	ne_target.setKSearch(10);
	ne_target.compute(*target_normals);
	pcl::concatenateFields(*target_cloud, *target_normals, *target_cloud_mid);
	pcl::copyPointCloud(*target_cloud_mid, *target_cloud_copy);

	//建立kd树
	pcl::search::KdTree<pcl::PointXYZLNormal>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZLNormal>);
	kdtree->setInputCloud(target_cloud_mid);

	//初始化变换矩阵等参数
	int iters = 0;
	double error = std::numeric_limits<double>::infinity();
	Eigen::Matrix3f R = Eigen::Matrix3f::Identity();
	Eigen::Vector3f T = Eigen::Vector3f::Zero();
	Eigen::Matrix4f H = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f H_final = H;
	Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();

	//开始迭代，直到满足条件
	clock_t start = clock();
	while (error > 0.0001 && iters < 100)
	{
    
    
		iters++;
		double last_error = error;
		double err = 0.0;
		pcl::transformPointCloud(*source_cloud_mid, *source_cloud_mid, H);
		std::vector<int>indexs(source_cloud_mid->size());

		//#pragma omp parallel for reduction(+:err) //采用openmmp加速
		for (int i = 0; i < source_cloud_mid->size(); ++i)
		{
    
    
			std::vector<int>index(1);
			std::vector<float>distance(1);
			kdtree->nearestKSearch(source_cloud_mid->points[i], 1, index, distance);
			err = err + sqrt(distance[0]);
			indexs[i] = index[0];
		}
		pcl::copyPointCloud(*target_cloud_copy, indexs, *target_cloud_mid);
		error = err / source_cloud->size();
		std::cout << "iters:" << iters << "  " << "error:" << error << std::endl;

		if (fabs(last_error - error) < 1e-6)
			break;

		Eigen::MatrixXf A(source_cloud_mid->size(), 6);
		for (size_t i = 0; i < source_cloud_mid->size(); i++)
		{
    
    
			float n_ix = target_cloud_mid->points[i].normal_x;
			float n_iy = target_cloud_mid->points[i].normal_y;
			float n_iz = target_cloud_mid->points[i].normal_z;
			float p_ix = source_cloud_mid->points[i].x;
			float p_iy = source_cloud_mid->points[i].y;
			float p_iz = source_cloud_mid->points[i].z;

			A.row(i) << n_iz * p_iy - n_iy * p_iz, n_ix* p_iz - n_iz * p_ix, n_iy* p_ix - n_ix * p_iy, n_ix, n_iy, n_iz;
		}

		Eigen::MatrixXf b(source_cloud_mid->size(), 1);
		for (size_t i = 0; i < source_cloud_mid->size(); i++)
		{
    
    
			float n_ix = target_cloud_mid->points[i].normal_x;
			float n_iy = target_cloud_mid->points[i].normal_y;
			float n_iz = target_cloud_mid->points[i].normal_z;
			float p_ix = source_cloud_mid->points[i].x;
			float p_iy = source_cloud_mid->points[i].y;
			float p_iz = source_cloud_mid->points[i].z;
			float q_ix = target_cloud_mid->points[i].x;
			float q_iy = target_cloud_mid->points[i].y;
			float q_iz = target_cloud_mid->points[i].z;

			b.row(i) << n_ix * q_ix + n_iy * q_iy + n_iz * q_iz - n_ix * p_ix - n_iy * p_iy - n_iz * p_iz;
		}

		//std::cout << A.transpose() * A << std::endl;
		//std::cout << (A.transpose() * A).inverse() << std::endl;
		//std::cout << (A.transpose() * b) << std::endl;
		Eigen::MatrixXf X = ((A.transpose() * A).inverse()) * (A.transpose() * b);
		std::cout << X << std::endl;

		float alpha = X(0, 0);
		float beta = X(1, 0);
		float gamma = X(2, 0);
		float x = X(3, 0);
		float y = X(4, 0);
		float z = X(5, 0);
		
		R << 1, -gamma, beta, gamma, 1, -alpha, -beta, alpha, 1;
		T << x, y, z;
		H << R, T, 0, 0, 0, 1;
		H_final = H * H_final; //更新变换矩阵	
	}
	transformation_matrix << H_final;

	clock_t end = clock();
	std::cout << end - start << "ms" << std::endl;
	std::cout << transformation_matrix << std::endl;

	//配准结果
	pcl::PointCloud<pcl::PointXYZ>::Ptr icp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud(*source_cloud, *icp_cloud, transformation_matrix);
	pcl::io::savePCDFileBinary("icp_cloud.pcd", *icp_cloud);

	可视化
	//pcl::visualization::PCLVisualizer viewer("registration Viewer");
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(source_cloud, 0, 255, 0); 	//原始点云绿色
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> tgt_h(target_cloud, 255, 0, 0); 	//目标点云红色
	//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_h(icp_cloud, 0, 0, 255); 	//匹配好的点云蓝色

	//viewer.setBackgroundColor(255, 255, 255);
	//viewer.addPointCloud(source_cloud, src_h, "source cloud");
	//viewer.addPointCloud(target_cloud, tgt_h, "target cloud");
	//viewer.addPointCloud(icp_cloud, final_h, "result cloud");
	//while (!viewer.wasStopped())
	//{
    
    
	//	viewer.spinOnce(100);
	//}

	system("pause");
	return 0;
}