迭代最近点算法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 massPoint3f p1, p2;int N = pts1.size();for (int i=0; i<N; i++){p1 += pts1[i];p2 += pts2[i];}p1 /= N;p2 /= N;// subtract COMvector<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^TEigen::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 WEigen::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::MatR = (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));
}

迭代最近点算法Iterative Closest Point（ICP）以及c++实现代码相关推荐

迭代最近点（Iterative Closest Point, ICP）算法及matlab实现
前言通常,使用RGB-D相机或是其他方法获取到物体的三维点云后,由于采集设备不同.拍摄视角不同等等因素的影响,即使是同一个物体所得到的点云也会有较大的差异,主要是旋转或者平移的变化.对于一组图像数据 ...
迭代最近点算法 Iterative Closest Points
研究生课程系列文章参见索引<在信科的那些课> 基本原理假定已给两个数据集P.Q, ,给出两个点集的空间变换f使他们能进行空间匹配.这里的问题是,f为一未知函数,而且两点集中的点数不一定相 ...
迭代最近点（Iterative Closest Point, ICP）算法
1. 定义 ICP(Iterative Closest Point,迭代最近点)算法是一种迭代计算方法,主要用于计算机视觉中深度图像的精确拼合,通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合 ...
迭代近邻算法Iterative Closest Point, ICP
参考的文章链接: https://zhuanlan.zhihu.com/p/35893884 https://blog.csdn.net/u014709760/article/details/9924 ...
绝对不可错过的图形学算法！迭代最近点算法——ICP算法
图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对 ...
ICP算法（Iterative Closest Point迭代最近点算法）
最近在做点云匹配,需要用c++实现ICP算法,下面是简单理解,期待高手指正. ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐 ...
VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）
1.Iterative Closest Points算法点云数据配准最经典的方法是迭代最近点算法(Iterative Closest Points,ICP).ICP算法是一个迭代的过程,每次迭代中对 ...
ICP（迭代最近点）算法推导详解
概述 ICP用来求解3D-3D的位姿估计问题.假设有一组配对好的3D点,例如对两幅RGB-D图像进行匹配,或者是EPnP方法中已知参考的在相机坐标系中的坐标和在世界坐标系的坐标(参考我PnP的博文): ...
吉林大学计算机科学与技术学院王欣,应用改进迭代最近点方法的点云数据配准...
摘要: An improved Iterative Closest Point (ICP) method based on the boundary feature points of the poi ...

迭代最近点算法Iterative Closest Point（ICP）以及c++实现代码

迭代最近点算法Iterative Closest Point（ICP）以及c++实现代码相关推荐

最新文章

热门文章