Eigen点云刚体变换：旋转、平移与坐标系变换

Eigen点云刚体变换代码详解

本篇详解以下代码，该代码使用Eigen库实现了点云的刚体变换，包括旋转和平移，最终实现了将点云从参考坐标系变换到主方向坐标系的功能。

Eigen::Matrix4f transformation(Eigen::Matrix4f::Identity());
transformation.block<3, 3>(0, 0) = eigen_vectors.transpose(); // R^(-1) = R^T
transformation.block<3, 1>(0, 3) = -1.f * (transformation.block<3, 3>(0, 0) * centroid.head<3>()); // t^(-1) = -R^T * t
pcl::PointCloud<PointType> transformed_cloud; // 变换后的点云
pcl::transformPointCloud(*cloudg, transformed_cloud, transformation);
PointType min_pt, max_pt; // 沿参考坐标系坐标轴的边界值
pcl::getMinMax3D(transformed_cloud, min_pt, max_pt);
const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap()); // 形心
// 参考坐标系到主方向坐标系的变换关系
/*const Eigen::Quaternionf qfinal(eigen_vectors);
const Eigen::Vector3f tfinal = eigen_vectors * mean_diag + centroid.head<3>(); */
const Eigen::Quaternionf qfinal(eigen_vectors.transpose());
const Eigen::Vector3f tfinal = -eigen_vectors.transpose() * mean_diag + centroid.head<3>();

代码解析

代码可拆解为以下步骤：

初始化变换矩阵:
- Eigen::Matrix4f transformation(Eigen::Matrix4f::Identity()); 创建一个4x4的变换矩阵transformation，并初始化为单位矩阵。
设置旋转矩阵:
- transformation.block<3, 3>(0, 0) = eigen_vectors.transpose(); 将eigen_vectors的转置赋值给transformation的前3x3部分，表示旋转矩阵 R 的逆矩阵 R^(-1)，这里利用了正交矩阵的性质：R^(-1) = R^T。
设置平移向量:
- transformation.block<3, 1>(0, 3) = -1.f * (transformation.block<3, 3>(0, 0) * centroid.head<3>()); 计算平移向量 t^(-1) 并赋值给transformation的第1到3行的第4列。
  - centroid.head<3>() 获取点云质心的前三个元素，表示平移向量 t。
  - 平移向量的计算公式为 t^(-1) = -R^T * t。
点云变换:
- pcl::PointCloud<PointType> transformed_cloud; 创建一个新的点云transformed_cloud用于存储变换后的点云。
- pcl::transformPointCloud(*cloudg, transformed_cloud, transformation); 使用PCL库的transformPointCloud函数，将输入点云cloudg按照变换矩阵transformation进行刚体变换，结果存储在transformed_cloud中。
计算边界值和形心:
- pcl::getMinMax3D(transformed_cloud, min_pt, max_pt); 计算变换后的点云在参考坐标系下的边界值，存储在min_pt和max_pt中。
- const Eigen::Vector3f mean_diag = 0.5f*(max_pt.getVector3fMap() + min_pt.getVector3fMap()); 计算变换后点云的形心坐标mean_diag。
参考坐标系到主方向坐标系的变换:
- const Eigen::Quaternionf qfinal(eigen_vectors.transpose()); 使用主方向向量构建四元数qfinal，表示参考坐标系到主方向坐标系的旋转关系。
- const Eigen::Vector3f tfinal = -eigen_vectors.transpose() * mean_diag + centroid.head<3>(); 计算参考坐标系到主方向坐标系的平移向量tfinal。

总结

这段代码实现了点云的刚体变换，通过构建变换矩阵，将点云从原始坐标系变换到目标坐标系。其中，旋转矩阵和平移向量分别由主方向向量和点云质心计算得到。最终，代码还计算了参考坐标系到主方向坐标系的变换关系，方便后续操作。