SVD奇异值分解求解位姿变换矩阵

一、SVD奇异值分解理论推导

SVD奇异值分解求解旋转矩阵和平移矩阵的理论推导可以参考如下博文：
1. SLAM常见问题(四)：求解ICP，利用SVD分解得到旋转矩阵
2. 机器学习中的矩阵方法04：SVD 分解
3. SVD分解求变换矩阵(C++版)
4. 利用SVD求得两个对应点集合的旋转矩阵R和转移矩阵t的数学推导
5. 计算两个对应点集之间的旋转矩阵R和转移矩阵T

二、求解注意事项

很多情况下通过SVD分解得到的矩阵并不是最终旋转矩阵的结果，而是反射矩阵，因此需要在最后得到结果后进行处理，下面简单介绍下旋转矩阵和反射矩阵，然后说明如果结果为反射矩阵的话，具如何处理才能得到旋转矩阵。

旋转矩阵和反射矩阵

1. 正交矩阵是旋转矩阵吗？有文档证明了旋转矩阵是正交矩阵，那反过来，正交矩阵都是旋转矩阵吗？
2. 旋转和反射
3. 反射矩阵（reflection matrix）推导

反射矩阵的处理方法

当你通过SVD分解得到“旋转矩阵”（此处还没验证是旋转矩阵还是反射矩阵）的结果R=V * U.t时，首先要通过矩阵的行列式判断是旋转矩阵还是反射矩阵。如果R的行列式值为1，那么恭喜你，得到的结果是旋转矩阵；如果R的行列式值是-1，那么你要进一步判断是在哪个轴（x, y, z对应R矩阵的0，1，2列）产生了反射效果，通常是在计算R=V * U.t之前，先取一个单位对角矩阵E，将产生反射效果的那一列的E取反，然后按照R=V * E * U.t求得最终结果，具体实现方法如下。

三、代码实例

bool SolveSVD(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr &src_cloud,
        const pcl::PointCloud<pcl::PointXYZ>::Ptr &dest_cloud,
        Eigen::Matrix3d &R, Eigen::Vector3d &T)
{
    // central SVD method based on two corresponded point sets
    if (src_cloud->empty() || dest_cloud->empty() ||
        src_cloud->size() != dest_cloud->size()) {
        std::cout << "error: solve SVD: input pointcloud size is not same or empty." << std::endl;
        return false;
    }

    // center of mass
    const size_t N = src_cloud->size();
    Eigen::Vector3d p_{0, 0, 0};
    Eigen::Vector3d q_{0, 0, 0};
    for (size_t i = 0; i < N; i++) {
        pcl::PointXYZ p = src_cloud->points[i];
        p_ += Eigen::Vector3d(p.x, p.y, p.z);

        pcl::PointXYZ q = dest_cloud->points[i];
        q_ += Eigen::Vector3d(q.x, q.y, q.z);
    }

    p_ /= N;
    q_ /= N;

    // regularize points
    std::vector<Eigen::Vector3d> pv_list, qv_list;
    for (size_t i = 0; i < N; i++) {
        pcl::PointXYZ p = src_cloud->points[i];
        Eigen::Vector3d pi = Eigen::Vector3d(p.x, p.y, p.z) - p_;
        pv_list.push_back(pi);

        pcl::PointXYZ q = dest_cloud->points[i];
        Eigen::Vector3d qi = Eigen::Vector3d(q.x, q.y, q.z) - q_;
        qv_list.push_back(qi);
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (size_t i = 0; i < N; i++) {
        W += pv_list[i] * qv_list[i].transpose();
    }

    for (size_t i = 0; i < W.size(); i++) {
        if (std::isnan(W(i))) {
            std::cout << "error: the input points are wrong, can't solve with SVD." << std::endl;
        }
    }

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    Eigen::Matrix3d E = Eigen::Matrix3d::Identity();
    Eigen::Matrix3d R_temp = V * (U.transpose());
    // 反射矩阵的处理
    if (R_temp.determinant() < 0) {
        double dvalue = 0.0f;
        dvalue = R_temp(0, 0);
        if (dvalue < 0) {
            E.row(0) *= R_temp.determinant();
        }

        dvalue = R_temp.block(0, 0, 2, 2).determinant();
        if (dvalue < 0) {
            E.row(1) *= R_temp.determinant();
        }

        dvalue = R_temp.block(0, 0, 3, 3).determinant();
        if (dvalue < 0) {
            E.row(2) *= R_temp.determinant();
        }
    }
    R = V * E * (U.transpose());
    T = q_ - R * p_;

    return true;
}