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

Question

我有一个点云，已知包含地面。这个点云的方向未知，且不在原点(0,0,0)上。我需要：

• 将floor_plane移动到XY平面上，让其位于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);

- pr4n

---

我也遇到了同样的问题，你提供的方法对我帮助很大。谢谢。 - Sunil Singh

你用这段代码实现了想要的效果吗？我没能做到，后来换了另一种方法，才成功了。 - pr4n

如果我想要旋转到另一个轴（例如y轴），我需要将xy_plane_normal_vector [1]更改为1.0，将xy_plane_normal_vector [2]更改为0.0吗？还需要更改其他什么吗？ - alltooconfusingthereforesleep

嗨@PranavPrakash，你得到的其他解决方案是什么？你能分享一下细节吗？谢谢！ - ManJan

@ManJan 我尝试了otna_ecnav下面提到的方法，它有效。 - pr4n

Answer 1

先进行翻译，再进行旋转操作。

检查θ的符号。

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

- Bruzzlee

Answer 2

如果有人感兴趣的话

代码中存在2个问题：

1. 需要对旋转向量进行归一化处理（只需在transform_2.rotate (Eigen::AngleAxisf (theta, rotation_vector));这行代码中调用rotation_vector.normalized()）
2. 需要对角度取负值（如前一个回答所建议的）。

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

- otna_ecnav