Point cloud registration 1-ICP algorithm principle code implementation

1. Statement:

As a beginner, I just started to contact point cloud registration. If there are any mistakes, I hope everyone can point them out. I will modify them in time and make progress together.

2. Need to know the theoretical knowledge in advance

The code only implements the theoretical part of the ICP algorithm, and other parts will be implemented later.

I am not familiar enough with the ICP algorithm, I recommend the following blogs (especially the second one):

3D point cloud registration--ICP algorithm principle and derivation- Zhihu (zhihu.com)

SLAM Frequently Asked Questions (4): Solve ICP, use SVD decomposition to get the rotation matrix | RealCat (gitee.io)

Not familiar enough with singular value decomposition, you can take a look at:

Singular value decomposition (SVD) - Zhihu (zhihu.com)

3. Implementation steps of ICP code algorithm

step1. Calculate the centroid of two sets of matching points, the position of each point is: { }, { }: $p_1,p_2,p_3,p_4.....$ $q_1,q_2,q_3,q_4......$

$\bar{p} =\frac{\sum_{i=0}^{n}p_i}{n}$ ， $\bar{q} =\frac{\sum_{i=0}^{n}q_i}{n}$

step2 . Get the centroid-free point set:

$x_{i}:=p_{i}-\bar{p},y_{i}=q_{i}-\bar{q},i=1,2...n$

step3 . Calculate the 3x3 proof of its singular decomposition money according to the formula:

$H = XY^{T}$

Where: X is the source point cloud matrix with the centroid removed, and Y is the target point cloud matrix with the centroid removed

step4 . Decompose S SVD 奇异值to get the rotation matrix R:

$R = VU^T$

The calculation formula of V is ( $S^TS$ a matrix composed of eigenvectors):

$V =Eigenvectors( H^TH)$

The calculation formula of U is ( $SS^T$ a matrix composed of eigenvectors):

$U = Eigenvectors(HH^T)$

step5 . Calculate the translation amount:

$t = \bar{q} -R\bar{p}$

4. Code implementation (according to step 3)

1. The main function (if you want to modify the position of the initial point cloud, you can modify the second for under the main function):

int main()
{
	MatrixXd input(n,m);
	MatrixXd output(n,m);
	//给数组赋值
	for (int i = 0; i < m; i++)
	{
		input(0,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		input(1,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		input(2,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		output(0, i) = input(0, i);
		output(1, i) = input(1, i);
		output(2, i) = input(2, i);
	}
	//打印初始点云
	cout << input << endl;

	//将点云的x坐标进行先前移动0.7个坐标
	for (int i = 0; i < m; i++)
	{
		output(0,i) = output(0,i) + 0.7f;
	}
	cout << "-----------------" << endl;
	//打印出输出点云
	cout << output << endl;
	//icp算法
	icp(input, output);
}

1.1 Results:

目标点云：
1.28125 828.125 358.688   764.5 727.531
577.094 599.031 917.438 178.281 525.844
197.938 491.375 842.562 879.531 311.281
向X轴平移0.7f个单位的点云：
1.98125 828.825 359.387   765.2 728.231
577.094 599.031 917.438 178.281 525.844
197.938 491.375 842.562 879.531 311.281

2. Calculate the centroid function of the matrix

MatrixXd computer3DCentroid(MatrixXd& input)
{
	float sum_x = 0;
	float sum_y = 0;
	float sum_z = 0;
	for (int i = 0; i < m ; i++)
	{
		sum_x += input(0, i);
		sum_y += input(1, i);
		sum_z += input(2, i);
	}
	sum_x = sum_x / m;
	sum_y = sum_y / m;
	sum_z = sum_z / m;
	MatrixXd Centroid(1,3);
	Centroid << sum_x, sum_y, sum_z;
	return Centroid;
}

2.1 Results:

目标矩阵的质心：
536.025 559.537 544.537
经旋转和平移矩阵的质心：
536.725 559.537 544.537

3. Decentrate the target point cloud matrix and the source point cloud matrix and calculate the rotation matrix R and translation T

		//计算目标矩阵的质心并去质心
		MatrixXd inCenterI = computer3DCentroid(Target);
		MatrixXd myinCenterI = inCenterI.transpose() * Sing;
		MatrixXd  MyInCenterI = Target - myinCenterI;
		//计算源矩阵的质心并去质心
		MatrixXd inCenterO = computer3DCentroid(Trans);
		MatrixXd myinCenterO = inCenterO.transpose() * Sing;
		MatrixXd MyInCenterO = Trans - myinCenterO;
		//计算R
		MatrixXd H = MyInCenterO * MyInCenterI.transpose();
		MatrixXd  U = H * H.transpose();
		EigenSolver<MatrixXd> es(U);
		MatrixXd  Ue = es.pseudoEigenvectors();
		MatrixXd V = H.transpose() *H;
		EigenSolver<MatrixXd> es1(V);
		MatrixXd  Ve = es1.pseudoEigenvectors();
		R = Ve * Ue.transpose();
		MatrixXd  gR = MatrixXd::Identity(n, n);
		gR(2, 2) = R.determinant();
		R = Ve *gR * Ue.transpose();
		cout << "旋转矩阵R为：" << endl;
		cout << R << endl;
		cout << "---------------------这里行列式一定要为1-----------------" << endl;
		std::cout << "旋转矩阵R行列式： " << R.determinant() << std::endl;
		cout << "---------------------------------------------------------" << endl;
		//计算T
		T = inCenterI - R * inCenterO;
		cout << "转移量T:" << endl << T << endl;
		T = T.transpose() * Sing;
		cout << "转移量T矩阵：" << endl << T << endl;

3.1 Results

旋转矩阵R为：
           1 -8.57647e-15 -7.82707e-15
 8.88178e-15            1 -3.88578e-15
 7.82707e-15  3.83027e-15            1
---------------------这里行列式一定要为1-----------------
旋转矩阵R行列式： 1
---------------------------------------------------------
转移量T:
  -0.699951 2.27374e-13 2.27374e-13
转移量T矩阵：
  -0.699951   -0.699951   -0.699951   -0.699951   -0.699951
2.27374e-13 2.27374e-13 2.27374e-13 2.27374e-13 2.27374e-13
2.27374e-13 2.27374e-13 2.27374e-13 2.27374e-13 2.27374e-13

4. Perform error calculation:

	Trans = R * Trans + T;
	err = Trans - Target;
	err2 = (err.cwiseProduct(err)).sum();
	cout << err2 << endl;

1.19151e-08

5. Print the proof of the last rotation:

 1.2813 828.125 358.688   764.5 727.531
577.094 599.031 917.438 178.281 525.844
197.938 491.375 842.563 879.531 311.281

Five, complete code

# include<iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
using namespace Eigen;
using namespace std;

# define m 5
# define n 3
//计算质心的函数
MatrixXd computer3DCentroid(MatrixXd& input)
{
	float sum_x = 0;
	float sum_y = 0;
	float sum_z = 0;
	for (int i = 0; i < m ; i++)
	{
		sum_x += input(0, i);
		sum_y += input(1, i);
		sum_z += input(2, i);
	}
	sum_x = sum_x / m;
	sum_y = sum_y / m;
	sum_z = sum_z / m;
	MatrixXd Centroid(1,3);
	Centroid << sum_x, sum_y, sum_z;
	return Centroid;
}


void icp(MatrixXd& Target, MatrixXd& Source)
{
	MatrixXd R = MatrixXd::Identity(n, n);
	cout << R << endl;
	MatrixXd T(n, m);
	MatrixXd Trans = R * Source + T;
	MatrixXd err = Trans - Target;
	float err2 = (err.cwiseProduct(err)).sum();
	while (err2 > 0.5)
	{
		MatrixXd Sing(1, 5);
		Sing << 1, 1, 1,1,1;
		//计算目标矩阵的质心并去质心
		MatrixXd inCenterI = computer3DCentroid(Target);
		MatrixXd myinCenterI = inCenterI.transpose() * Sing;
		MatrixXd  MyInCenterI = Target - myinCenterI;
		//计算源矩阵的质心并去质心
		MatrixXd inCenterO = computer3DCentroid(Trans);
		MatrixXd myinCenterO = inCenterO.transpose() * Sing;
		MatrixXd MyInCenterO = Trans - myinCenterO;
		//计算R
		MatrixXd H = MyInCenterO * MyInCenterI.transpose();
		MatrixXd  U = H * H.transpose();
		EigenSolver<MatrixXd> es(U);
		MatrixXd  Ue = es.pseudoEigenvectors();
		MatrixXd V = H.transpose() *H;
		EigenSolver<MatrixXd> es1(V);
		MatrixXd  Ve = es1.pseudoEigenvectors();
		R = Ve * Ue.transpose();
		MatrixXd  gR = MatrixXd::Identity(n, n);
		gR(2, 2) = R.determinant();
		R = Ve *gR * Ue.transpose();
		cout << "旋转矩阵R为：" << endl;
		cout << R << endl;
		cout << "---------------------这里行列式一定要为1-----------------" << endl;
		std::cout << "旋转矩阵R行列式： " << R.determinant() << std::endl;
		cout << "---------------------------------------------------------" << endl;
		//计算T
		T = inCenterI - R * inCenterO;
		cout << "转移量T:" << endl << T << endl;
		T = T.transpose() * Sing;
		cout << "转移量T矩阵：" << endl << T << endl;
		Trans = R * Trans + T;
		err = Trans - Target;
		err2 = (err.cwiseProduct(err)).sum();
		cout <<"误差为：" << err2 << endl;
	}
	cout << "Trans:" << endl;
	cout << Trans << endl;
	
}
int main()
{
	MatrixXd input(n,m);
	MatrixXd output(n,m);
	//给数组赋值
	for (int i = 0; i < m; i++)
	{
		input(0,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		input(1,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		input(2,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		output(0, i) = input(0, i);
		output(1, i) = input(1, i);
		output(2, i) = input(2, i);
	}
	//打印初始点云
	cout << "目标点云：" << endl;
	cout << input << endl;
	//将点云的x坐标进行先前移动0.7个坐标
	for (int i = 0; i < m; i++)
	{
		output(0,i) = output(0,i) + 0.7f;
		output(1, i) = output(1, i) + 0.1f;
	}

	//打印出输出点云
	cout << "向X轴平移0.7f个单位的点云：" << endl;
	cout << output << endl;
	//icp算法
	icp(input, output);
}

Six points: Supplement:

If we change the second for loop under the main function to reassign the y coordinate of the point cloud and move 500 in the y direction, we will find that the ICP will not converge, so its shortcomings are more obvious, requiring an initial registration, and it is easy to fall into local optimum.

int main()
{
	MatrixXd input(n,m);
	MatrixXd output(n,m);
	//给数组赋值
	for (int i = 0; i < m; i++)
	{
		input(0,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		input(1,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		input(2,i) = 1024 * rand() / (RAND_MAX + 1.0f);
		output(0, i) = input(0, i);
		output(1, i) = input(1, i);
		output(2, i) = input(2, i);
	}
	//打印初始点云
	cout << "目标点云：" << endl;
	cout << input << endl;
	//将点云的x坐标进行先前移动0.7个坐标
    //点云y坐标重新赋值且向y方向移动500f
	for (int i = 0; i < m; i++)
	{
		output(0,i) = output(0,i) + 0.7f;
		output(1, i) = output(1, i) + 0.7f;
	}

	//打印出输出点云
	cout << "向X轴平移0.7f个单位的点云：" << endl;
	cout << output << endl;
	//icp算法
	icp(input, output);
}