改写代码使得主方向坐标系与参考坐标系重合的变换矩阵并将输入点云进行变换EigenMatrix4f transformationEigenMatrix4fIdentity; transformationblock3 30 0 = eigen_vectorstranspose; R^-1 = R^T transformat

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 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.transpose()); const Eigen::Vector3f tfinal = -eigen_vectors.transpose() * mean_diag + centroid.head<3>();

// 显示结果 pcl::visualization::PCLVisualizer viewer; pcl::visualization::PointCloudColorHandlerCustompcl::PointXYZRGB single_color(cloudg, 255, 255, 255); viewer.addPointCloudpcl::PointXYZRGB(cloudg, single_color, "cloud84")