pointcloud 转ptr_math – 在PointCloud中旋转和转换平面到XY平面和原点

我有一个已知包含地板的点云.这是在一个未知的方向,而不是在原点(0,0,0).我必须

>将floor_plane移动到XY平面,使floor_plane位于XY平面上

>将floor_plane的质心移动到原点(0,0,0).

我对上述方法的处理方法是：

>从RANSAC获取floor_plane的平面系数.前三个系数对应于floor_plane的法线.

>生成XY平面的法线向量.这将是x = 0,y = 0和z = 1.

>计算地平面法线与xy平面法线的交叉积,得到旋转矢量(轴),即单位矢量.

>计算旋转角度.平面之间的角度等于法线之间的角度.从点积的定义,我们可以提取两个法向量之间的角度.在XY平面的情况下,它等于theta = acos(C / sqrt(A ^ 2 B ^ 2 C ^ 2),其中A,B,C是floor_plane的前三个系数.

>生成旋转矩阵(3×3)或四元数.在维基百科中寻找公式.

>找到floor_plane的质心.否定它们以生成翻译

>使用transformPointCloud(cloud,transformed,transformationMatrix)简单地应用转换

我使用PointCloud Library的代码如下.它无法执行所需的转换,我不知道为什么.有线索吗？

// Find the planar coefficients for floor plane

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);

pcl::PointIndices::Ptr floor_inliers (new pcl::PointIndices);

pcl::SACSegmentation<:pointxyzrgb> seg;

seg.setOptimizeCoefficients (true);

seg.setModelType (pcl::SACMODEL_PLANE);

seg.setMethodType (pcl::SAC_RANSAC);

seg.setDistanceThreshold (0.01);

seg.setInputCloud (floor_plane);

seg.segment (*floor_inliers, *coefficients);

std::cerr << "Floor Plane Model coefficients: " << coefficients->values[0] << " "

<< coefficients->values[1] << " "

<< coefficients->values[2] << " "

<< coefficients->values[3] << std::endl;

Eigen::Matrix floor_plane_normal_vector, xy_plane_normal_vector, rotation_vector;

floor_plane_normal_vector[0] = coefficients->values[0];

floor_plane_normal_vector[1] = coefficients->values[1];

floor_plane_normal_vector[2] = coefficients->values[2];

std::cout << floor_plane_normal_vector << std::endl;

xy_plane_normal_vector[0] = 0.0;

xy_plane_normal_vector[1] = 0.0;

xy_plane_normal_vector[2] = 1.0;

std::cout << xy_plane_normal_vector << std::endl;

rotation_vector = xy_plane_normal_vector.cross (floor_plane_normal_vector);

std::cout << "Rotation Vector: "<< rotation_vector << std::endl;

float theta = acos(floor_plane_normal_vector.dot(xy_plane_normal_vector)/sqrt( pow(coefficients->values[0],2)+ pow(coefficients->values[1],2) + pow(coefficients->values[2],2)));

Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();

transform_2.translation() << 0, 0, 30;

transform_2.rotate (Eigen::AngleAxisf (theta, rotation_vector));

std::cout << "Transformation matrix: " << std::endl << transform_2.matrix() << std::endl;

pcl::transformPointCloud (*cloud, *centeredCloud, transform_2);