迭代最近點算法Iterative Closest Point（ICP）以及c++實現代碼

有兩組對應的點集（corresponding point sets）:

求歐式變換  使得：

ICP 算法基於最小二乘法進行迭代計算，使得誤差平方和達到極小值：

可通過以下三個步驟進行求解：

（1）定義兩組點的質心，簡化目標函數

 

 

交叉項部分 在求和之後零，因此目標函數簡化爲：

第一項只與旋轉矩陣R 有關，只要獲得 R ，令第二項爲零就能求得 t 。 

（2）計算每個點的去質心座標，計算旋轉矩陣R

其中，第二項 與R 無關，只有第三項與 R有關。因而，目標函數變爲： 

通過 Singular Value Decomposition (SVD) 來進行求解，先定義矩陣：

W是一個 矩陣，對W 進行 SVD 分解，得： 

其中， 爲奇異值組成的對角矩陣，當 W滿秩時，R 爲： 

（3）計算平移矩陣t

 

 （4）slam視覺十四講ICP代碼：

void pose_estimation_3d3d(const vector<Point3f>& pts1,
                          const vector<Point3f>& pts2,
                          Mat& R, Mat& t)
{
    // center of mass
    Point3f p1, p2;
    int N = pts1.size();
    for (int i=0; i<N; i++)
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 /= N;
    p2 /= N;

    // subtract COM
    vector<Point3f> q1(N), q2(N);
    for (int i=0; i<N; i++)
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for (int i=0; i<N; i++)
    {
        W += Eigen::Vector3d(q1[i].x, q1[i].y, q1[i].z) * Eigen::Vector3d(q2[i].x,
                q2[i].y, q2[i].z).transpose();
    }
    cout << "W=" << W << endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    cout << "U=" << U << endl;
    cout << "V=" << V << endl;

    Eigen::Matrix3d R_ = U * (V.transpose());
    Eigen::Vector3d t_ = Eigen::Vector3d(p1.x, p1.y, p1.z) - R_ * Eigen::Vector3d(p2.x, p2.y, p2.z);

    // convert to cv::Mat
    R = (Mat_<double>(3, 3) <<
            R_(0, 0), R_(0, 1), R_(0,2),
            R_(1, 0), R_(1, 1), R_(1,2),
            R_(2, 0), R_(2, 1), R_(2,2));
    t = (Mat_<double>(3, 1) << t_(0, 0), t_(1, 0), t_(2, 0));
}