PCL 点云点对点映射关系计算

一、简介

  已知点云中的的三个点，然后计算目标点与已知点云中的变换矩阵。刚开始的我用自己的方法写了计算AX=B的方程计算得出的结果是有问题的，不管是不是采用归一化（可能是我的方法有问题），因此个人还是采用了ICP的方式计算了变换矩阵。

二、自己的矩阵

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;

}

但是，这个方法是错误的，记录一下

三、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);
}