svd求解转换矩阵的c++代码

以下是使用Eigen库实现SVD求解转换矩阵的C++代码：

#include <Eigen/Dense>

using namespace Eigen;

// source_pts和target_pts分别为源点云和目标点云
Matrix4d computeTransformation(const MatrixXd& source_pts, const MatrixXd& target_pts)
{
    // 对点云进行中心化
    Vector3d source_centroid = source_pts.rowwise().mean();
    Vector3d target_centroid = target_pts.rowwise().mean();
    MatrixXd centered_source = source_pts;
    centered_source.rowwise() -= source_centroid.transpose();
    MatrixXd centered_target = target_pts;
    centered_target.rowwise() -= target_centroid.transpose();

    // 计算协方差矩阵
    Matrix3d covariance_matrix = centered_source.transpose() * centered_target;

    // 使用SVD分解求解旋转矩阵R和平移向量t
    JacobiSVD<MatrixXd> svd(covariance_matrix, ComputeThinU | ComputeThinV);
    Matrix3d R = svd.matrixV() * svd.matrixU().transpose();
    Vector3d t = target_centroid - R * source_centroid;

    // 组合成变换矩阵
    Matrix4d transformation = Matrix4d::Identity();
    transformation.block<3, 3>(0, 0) = R;
    transformation.block<3, 1>(0, 3) = t;

    return transformation;
}

其中，Matrix4d代表4x4矩阵，MatrixXd代表任意大小的矩阵，Vector3d代表3维向量。函数返回的是4x4的变换矩阵，可以通过将其作用于源点云得到变换后的点云。