PCL-根据向量做点云旋转

旋转前角度为该拟合平面方程的(A,B,C)的值，旋转后角度为（0,0,1)。首先把两个向量单位化，再通过点积可直接求得向量的夹角θ；再根据向量叉积求得垂直于这两个向量所在平面的法向量，即旋转轴，将旋转轴单位化得到单位向量k。最后将夹角θ和旋转轴的单位向量k代入罗德里格斯旋转公式，即可得到对应的旋转矩阵。最后利用pcl的旋转，实际上就是现有矩阵乘以旋转矩阵，这样就能得到旋转后的图像了。

Eigen::Matrix4f CreateRoatationMatrix(Eigen::Vector3f angle_before, Eigen::Vector3f angle_after)
{
   

	angle_before.normalize();
	angle_after.normalize();
	float angle = acos(angle_before.dot(angle_after));//点积，得到两向量的夹角
	Eigen::Vector3f p_rotate = angle_before.cross(angle_after);//叉积，得到的还是向量
	p_rotate.normalize();//该向量的单位向量，即旋转轴的单位向量
	Eigen::Matrix4f rotationMatrix = Eigen::Matrix4f::Identity();