PCL point cloud point-to-point mapping relationship calculation

1. Introduction

  Three points in the known point cloud are known, and then the transformation matrix between the target point and the known point cloud is calculated. At the beginning, I used my own method to write the equation for calculating AX=B. The calculated result is problematic, whether it is normalized or not (maybe there is a problem with my method), so I still use ICP. way to calculate the transformation matrix.

2. Your own matrix

void  GetMatrixBy6Points(Eigen::Vector4f p1, Eigen::Vector4f p2, Eigen::Vector4f p3, Eigen::Vector4f p11, Eigen::Vector4f p22, Eigen::Vector4f p33, 
	Eigen::Matrix3f  & r)

{

	p1.normalize();
	p2.normalize();
	p3.normalize();
	p11.normalize();
	p22.normalize();
	p33.normalize();
	// 计算点与点之间的刚性变换
	Eigen::Matrix3f  A = Eigen::Matrix3f::Identity();
	Eigen::Matrix3f  B = Eigen::Matrix3f::Identity();
	// 分别给
	A(0,0)= p1[0];	A(0, 1) = p1[1];	A(0, 2) = p1[2];
	A(1, 0) = p2[0];	A(1, 1) = p2[1];	A(1, 2) = p2[2];
	A(2, 0) = p3[0];	A(2, 1) = p3[1];	A(2, 2) = p3[2];

	//  
	B(0, 0) = p11[0];	B(0, 1) = p11[1];	B(0, 2) = p11[2];
	B(1, 0) = p22[0];	B(1, 1) = p22[1];	B(1, 2) = p22[2];
	B(2, 0) = p33[0];	B(2, 1) = p33[1];	B(2, 2) = p33[2];


     // 计算 AX=B
     r = A.fullPivLu().solve(B);
	 // 归一化


	  计算矩阵A的逆
	 r = A.inverse()*B;

}

However, this method is wrong , record it

3. Calculate the matrix by ICP

//利用SVD方法求解变换矩阵  
void MyVeroAlgroithm::GetMatrixByPoint2Point(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out,
	pcl::registration::TransformationEstimationSVD<pcl::PointXYZRGB, pcl::PointXYZRGB>::Matrix4 &transformation)
{
	
	pcl::registration::TransformationEstimationSVD<pcl::PointXYZRGB, pcl::PointXYZRGB> TESVD;
	// 计算点云之间的关系
	TESVD.estimateRigidTransformation(*cloud, *cloud_out, transformation);
}