将平面旋转和转换为XY平面并在PointCloud中生成

Question

我有一个已知包含地板的点云。这是在一个未知的方向，而不是在原点（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的前三个系数。
• 生成旋转矩阵（3x3）或四元数。在维基百科中寻找公式。
• 找到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<pcl::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<float, 1, 3> 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);

Answer 1

首先进行翻译，然后进行旋转。

检查theta的标志

Eigen::Vector3f rotation_vector = xy_plane_normal_vector.cross(floor_plane_normal_vector);
float theta = -atan2(rotation_vector.norm(), xy_plane_normal_vector.dot(floor_plane_normal_vector));

Answer 2

如果有人有兴趣

代码中有2个问题

1. 旋转向量需要规范化（只需调用rotation_vector.normalized（））
2. 角度需要否定（如前面的答案所示）。

感谢您发布代码，它帮助我快速完成。