平面到 XY 平面的旋转和变换以及点云中的原点

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 库的代码如下。它无法执行所需的转换，我不确定为什么。有什么线索吗？

      // 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 个问题

旋转向量需要归一化（只需调用rotation_vector.normalized()）
角度需要取反（如前一个答案所建议的）。

感谢您发布代码，它帮助我快速完成了这项工作。

平面到 XY 平面的旋转和变换以及点云中的原点

Rotation and Transformation of a plane to XY plane and origin in PointCloud

math

3d

rotation

point-cloud-library

eigen3