PCL公共点转换计算平移参数以及旋转参数

std::tuple<Eigen::Vector3d, Eigen::Vector3d> RotateColudMatrix(pcl::PointCloud<pcl::PointXYZ>::Ptr src, pcl::PointCloud<pcl::PointXYZ>::Ptr dst)
{
    assert(src.size() == dst.size());

    // 创建TransformationEstimationSVD对象
    pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> te;

    // 计算两个点云之间的刚性变换
    Eigen::Matrix4f transformation;
    te.estimateRigidTransformation(*src, *dst, transformation);

    // 平移参数
    Eigen::Vector3d translation = transformation.block<3, 1>(0, 3).cast<double>();
    std::cout << "tx:" << translation[0] << std::endl;
    std::cout << "ty:" << translation[1] << std::endl;
    std::cout << "tz:" << translation[2] << std::endl;
    // 提取旋转矩阵
    Eigen::Matrix3d rotation = transformation.block<3, 3>(0, 0).cast<double>();
    // 转为rx ry rf
    Eigen::Vector3d eulerAngle = rotation.eulerAngles(0, 1, 2);
    std::cout << "rx rad:" << eulerAngle << std::endl;
    std::cout << "ry rad:" << eulerAngle << std::endl;
    std::cout << "rz rad:" << eulerAngle << std::endl;
    return std::make_tuple(eulerAngle, translation);
}