Point-to-plane ICP algorithm

Reference:
Point cloud registration algorithm ICP and its various variants, worth reading! Derivation of point cloud registration
based on Gauss Newton's point-point, point-line, point-to-plane ICP process : Point-to-Point and Point-to-Plane formula derivation in rigid ICP ==> help you code implementation

Iterative closest point (ICP) algorithms using point-plane error metrics have been shown to converge faster than algorithms using point-point error metrics. At each iteration of the ICP algorithm, the relative pose change that produces the minimum point-to-plane error is usually resolved using standard non-linear least squares. For example the Levenberg-Marquardt method. When using the point-to-plane error metric, what is minimized is the sum of the squared distances between each source point and the tangent plane of its corresponding target point.

The minimized loss function can be written as:

where is the source point, is the corresponding destination point, and is the unit normal vector at. The advantages and disadvantages of this method are also obvious: first, the point-to-plane cost function allows flat areas to slide against each other; point-to-plane usually converges faster than point-to-point; the number of iterations is less; Slower and requires surface normals. There is no analytical solution for this formula, we can only use the nonlinear least squares method to solve it. In order to speed up the solution, some researchers found that when the relative direction between the two input surfaces is small, an approximate method can be used to effectively solve it. Linear least squares approximation to nonlinear optimization problems.
The reason it's hard to optimize is that R is too complex to optimize. In each iteration, we assume that the rotation angle is very small, that is,

then the rotation matrix can be approximately expressed as:

Rewrite the loss function as:

where:

such a nonlinear loss function is approximately linear, and the optimal solution x of each iteration is :

After multiple iterations until the algorithm converges.

#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;
}