点云配准之icp的简单实现

         C++实现经典的点云配准算法——ICP，根据高博视觉14讲的推导（具体过程看书，还是很好理解的），ICP的计算过程如下所示：

具体的C++代码实现如下：

void pose_estimation_3d3d (
    const vector& pts1,
    const vector& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i