math - 平面到 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)简单地应用转换

我使用点云库的代码如下。它无法执行所需的转换，我不确定为什么。有什么线索吗？

      // 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));