迭代最近邻ICP算法

ICP算法主要用于点云精配准，精度很高，但是相应的缺点就是迭代过程中容易陷入局部极值。具体的ICP算法推导过程很多书上都有，就不再详述了，此次仿真用的是SVD分解的方法。

%%% icp.mclear;
close all;
clc;data_source=load('satellite.txt');
data_source=data_source';
theta=4;  %旋转角度（此处只有绕z轴旋转）
t=[2,1.6,7];   %平移向量
[data_target,T0]=rotate(data_source,theta,t);  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%绘制两幅原始图像
x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,'b');
hold on;
scatter3(x2,y2,z2,'r');
hold off;T_final=eye(4,4);   %旋转矩阵初始值
iteration=0;
Rf=T_final(1:3,1:3);
Tf=T_final(1:3,4);
data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %初次更新点集（代表粗配准结果）
err=1;
while(err>0.001)iteration=iteration+1;    %迭代次数disp(['迭代次数ieration=',num2str(iteration)]);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%利用欧式距离找出对应点集k=size(data_target,2);for i = 1:kdata_q1(1,:) = data_source(1,:) - data_target(1,i);    % 两个点集中的点x坐标之差data_q1(2,:) = data_source(2,:) - data_target(2,i);    % 两个点集中的点y坐标之差data_q1(3,:) = data_source(3,:) - data_target(3,i);    % 两个点集中的点z坐标之差distance = data_q1(1,:).^2 + data_q1(2,:).^2 + data_q1(3,:).^2;  % 欧氏距离[min_dis, min_index] = min(distance);   % 找到距离最小的那个点data_mid(:,i) = data_source(:,min_index);   % 将那个点保存为对应点error(i) = min_dis;     % 保存距离差值end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%去中心化data_target_mean=mean(data_target,2);data_mid_mean=mean(data_mid,2);data_target_c=data_target-data_target_mean*ones(1,size(data_target,2));data_mid_c=data_mid-data_mid_mean*ones(1,size(data_mid,2));%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%SVD分解W=zeros(3,3);for j=1:size(data_target_c,2)W=W+data_mid_c(:,j)*data_target_c(:,j)';end[U,S,V]=svd(W);Rf=U*V';Tf=data_mid_mean-Rf*data_target_mean;err=mean(error);T_t=[Rf,Tf];T_t=[T_t;0,0,0,1];T_final=T_t*T_final;   %更新旋转矩阵disp(['误差err=',num2str(err)]);disp('旋转矩阵T=');disp(T_final);data_target=Rf*data_target+Tf*ones(1,size(data_target,2));    %更新点集if iteration>=200breakend
enddisp(inv(T0));  %旋转矩阵真值x1=data_source(1,:);
y1=data_source(2,:);
z1=data_source(3,:);
x2=data_target(1,:);
y2=data_target(2,:);
z2=data_target(3,:);
figure;
scatter3(x1,y1,z1,'b');
hold on;
scatter3(x2,y2,z2,'r');
hold off;

%% rotate.mfunction [data_q,T] = rotate(data,theta,t)theta=-theta/180*pi;
T=[cos(theta),sin(theta),0,t(1);-sin(theta),cos(theta),0,t(2);0,0,1,t(3);0,0,0,1];            %旋转矩阵rows=size(data,2);
rows_one=ones(1,rows);
data=[data;rows_one];    %化为齐次坐标data_q=T*data;
data_q=data_q(1:3,:);    %返回三维坐标

仿真结果（配准前和配准后）：

旋转矩阵（真值和配准结果）

上面是比较好的配准结果，下面来一组局部极值的情况。

仿真结果（配准前和配准后）：

旋转矩阵（真值和配准结果）

从上面两组仿真结果可以明显看到，ICP算法在一定情况下精度很高，很适合用来精配准，但是缺点在于需要很好的迭代初值，这个直接关系到配准结果的准确性，因此ICP最好不要单独使用，应该在该算法之前进行粗配准（该方法很多，比如利用特征点等）。另外，由于不知道两堆点云的点对对应关系，在此使用的是寻找最近点的方法，该方法最大的不足时运算量很大，因此如果在C++中使用，可以考虑采用KD-tree的存储方法提高搜索效率或者想办法进行高效率点云配对。

图像配准是图像处理研究领域中的一个典型问题和技术难点，其目的在于比较或融合针对同一对象在不同条件下获取的图像，例如图像会来自不同的采集设备，取自不同的时间，不同的拍摄视角等等，有时也需要用到针对不同对象的图像配准问题。具体地说，对于一组图像数据集中的两幅图像，通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。一个经典的应用是场景的重建，比如说一张茶几上摆了很多杯具，用深度摄像机进行场景的扫描，通常不可能通过一次采集就将场景中的物体全部扫描完成，只能是获取场景不同角度的点云，然后将这些点云融合在一起，获得一个完整的场景。

ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。如下图所示，PR（红色点云）和RB（蓝色点云）是两个点集，该算法就是计算怎么把PB平移旋转，使PB和PR尽量重叠。

　用数学语言描述如下，即ICP算法的实质是基于最小二乘法的最优匹配，它重复进行“确定对应关系的点集→计算最优刚体变换”的过程，直到某个表示正确匹配的收敛准则得到满足。
　

如果知道正确的点对应，那么两个点集之间的相对变换（旋转、平移）就可以求得封闭解。

首先计算两个点集X和P的质心，分别为μx和μp

然后在两个点集中分别减去对应的质心（Subtract the corresponding center of mass from every point in the two point sets before calculating the transformation）

目标函数E(R,t)的优化是ICP算法的最后一个阶段。在求得目标函数后，采用什么样的方法来使其收敛到最小，也是一个比较重要的问题。求解方法有基于奇异值分解的方法、四元数方法等。

如果初始点“足够近”，可以保证收敛性

ICP算法优点：

可以获得非常精确的配准效果
不必对处理的点集进行分割和特征提取
在较好的初值情况下，可以得到很好的算法收敛性
ICP算法的不足之处：

在搜索对应点的过程中，计算量非常大，这是传统ICP算法的瓶颈

标准ICP算法中寻找对应点时，认为欧氏距离最近的点就是对应点。这种假设有不合理之处，会产生一定数量的错误对应点

针对标准ICP算法的不足之处，许多研究者提出ICP算法的各种改进版本，主要涉及如下所示的6个方面。

标准ICP算法中，选用点集中所有的点来计算对应点，通常用于配准的点集元素数量都是非常巨大的，通过这些点集来计算，所消耗的时间很长。在后来的研究中，提出了各种方法来选择配准元素，这些方法的主要目的都是为了减小点集元素的数目，即如何选用最少的点来表征原始点集的全部特征信息。在点集选取时可以：1.选取所有点；2.均匀采样（Uniform sub-sampling ）；3.随机采样（Random sampling）；4.按特征采样（Feature based Sampling ）；5.法向空间均匀采样（Normal-space sampling），如下图所示，法向采样保证了法向上的连续性（Ensure that samples have normals distributed as uniformly as possible）

基于特征的采样使用一些具有明显特征的点集来进行配准，大量减少了对应点的数目。

点集匹配上有：最近邻点（Closet Point）

法方向最近邻点（Normal Shooting）

投影法（Projection）

import numpy as npdef best_fit_transform(A, B):
‘’’
Calculates the least-squares best-fit transform between corresponding 3D points A->B
Input:
A: Nx3 numpy array of corresponding 3D points
B: Nx3 numpy array of corresponding 3D points
Returns:
T: 4x4 homogeneous transformation matrix
R: 3x3 rotation matrix
t: 3x1 column vector‘’’
assert len(A) == len(B)# translate points to their centroids
centroid_A = np.mean(A, axis=0)
centroid_B = np.mean(B, axis=0)
AA = A - centroid_A
BB = B - centroid_B# rotation matrix
W = np.dot(BB.T, AA)
U, s, VT = np.linalg.svd(W)
R = np.dot(U, VT)# special reflection case
if np.linalg.det(R) < 0:VT[2,:] *= -1R = np.dot(U, VT)# translation
t = centroid_B.T - np.dot(R,centroid_A.T)# homogeneous transformation
T = np.identity(4)
T[0:3, 0:3] = R
T[0:3, 3] = treturn T, R, t

def nearest_neighbor(src, dst):
‘’’
Find the nearest (Euclidean) neighbor in dst for each point in src
Input:
src: Nx3 array of points
dst: Nx3 array of points
Output:
distances: Euclidean distances (errors) of the nearest neighbor
indecies: dst indecies of the nearest neighbor
‘’’
indecies = np.zeros(src.shape[0], dtype=np.int)
distances = np.zeros(src.shape[0])
for i, s in enumerate(src):min_dist = np.inffor j, d in enumerate(dst):dist = np.linalg.norm(s-d)if dist < min_dist:min_dist = distindecies[i] = jdistances[i] = dist
return distances, indecies

def icp(A, B, init_pose=None, max_iterations=50, tolerance=0.001):
‘’’
The Iterative Closest Point method
Input:
A: Nx3 numpy array of source 3D points
B: Nx3 numpy array of destination 3D point
init_pose: 4x4 homogeneous transformation
max_iterations: exit algorithm after max_iterations
tolerance: convergence criteria
Output:
T: final homogeneous transformation
distances: Euclidean distances (errors) of the nearest neighbor
# make points homogeneous, copy them so as to maintain the originals
src = np.ones((4,A.shape[0]))
dst = np.ones((4,B.shape[0]))
src[0:3,:] = np.copy(A.T)
dst[0:3,:] = np.copy(B.T)# apply the initial pose estimation
if init_pose is not None:src = np.dot(init_pose, src)prev_error = 0for i in range(max_iterations):# find the nearest neighbours between the current source and destination pointsdistances, indices = nearest_neighbor(src[0:3,:].T, dst[0:3,:].T)# compute the transformation between the current source and nearest destination pointsT,_,_ = best_fit_transform(src[0:3,:].T, dst[0:3,indices].T)# update the current source
# refer to "Introduction to Robotics" Chapter2 P28. Spatial description and transformationssrc = np.dot(T, src)# check errormean_error = np.sum(distances) / distances.sizeif abs(prev_error-mean_error) < tolerance:breakprev_error = mean_error# calculcate final tranformation
T,_,_ = best_fit_transform(A, src[0:3,:].T)return T, distancesif __name__ == "__main__":
A = np.random.randint(0,101,(20,3))    # 20 points for testrotz = lambda theta: np.array([[np.cos(theta),-np.sin(theta),0],[np.sin(theta),np.cos(theta),0],[0,0,1]])
trans = np.array([2.12,-0.2,1.3])
B = A.dot(rotz(np.pi/4).T) + transT, distances = icp(A, B)np.set_printoptions(precision=3,suppress=True)
print T‘’’

上面代码创建一个源点集A（在0-100的整数范围内随机生成20个3维数据点），然后将A绕Z轴旋转45°并沿XYZ轴分别移动一小段距离，得到点集B。结果如下，可以看出ICP算法正确的计算出了变换矩阵。

1.首先需要明确公式里的变换是T（P→X），即通过旋转和平移把点集P变换到X。我们这里求的变换是T（A→B），要搞清楚对应关系。

2.本例只用了20个点进行测试，ICP算法在求最近邻点的过程中需要计算20×20次距离并比较大小。如果点的数目巨大，那算法的效率将非常低。

3.两个点集的初始相对位置对算法的收敛性有一定影响，最好在“足够近”的条件下进行ICP配准。

ICP算法推导

常用的求解 R 和 t 的方法有：
SVD
非线性优化

非线性优化的描述比较繁琐，下面只介绍SVD方法。为了后面能使用SVD，我们需要在数学上做一点小小的变形。首先定义两组点云的质心（center of mass）为

并作出如下处理：

参考文献：ICP算法

迭代最近邻ICP算法相关推荐

icp点云匹配迭代最近邻算法
一.含义: 1.icp算法能够使两个不同坐标系下的点集匹配到一个坐标系中,这个过程就是配准,配准的操作就是找到从坐标系1变换到坐标系2的刚性变换. 2.icp的本质就是配准,但有不同的配准方案,icp ...
MATLAB点云处理：读取、展示、最近邻、ICP算法求取转移矩阵、旋转
MATLAB中关于点云的几个函数的简单应用.作者使用的是MATLAB R2015b,这几个函数应该是在Computer Vison包里. 全文都是作者自己结合MATLAB文档的理解,欢迎指教. 1. ...
VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）
1.Iterative Closest Points算法点云数据配准最经典的方法是迭代最近点算法(Iterative Closest Points,ICP).ICP算法是一个迭代的过程,每次迭代中对 ...
绝对不可错过的图形学算法！迭代最近点算法——ICP算法
图像配准是图像处理研究领域中的一个典型问题和技术难点,其目的在于比较或融合针对同一对象在不同条件下获取的图像,例如图像会来自不同的采集设备,取自不同的时间,不同的拍摄视角等等,有时也需要用到针对不同对 ...
ICP算法（Iterative Closest Point迭代最近点算法）
最近在做点云匹配,需要用c++实现ICP算法,下面是简单理解,期待高手指正. ICP算法能够使不同的坐标下的点云数据合并到同一个坐标系统中,首先是找到一个可用的变换,配准操作实际是要找到从坐标系1到坐 ...
迭代最近点算法Iterative Closest Point（ICP）以及c++实现代码
有两组对应的点集(corresponding point sets): 求欧式变换使得: ICP 算法基于最小二乘法进行迭代计算,使得误差平方和达到极小值: 可通过以下三个步骤进行求解: (1)定 ...
在医学图像分析中使用ICP算法进行点云配准
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文转载自「计算机视觉工坊」,该公众号重点在于介绍深度学习.智能驾驶等领域,一个小众的公众号. 论文标 ...
ICP算法与Kdtree
一.ICP算法输入是两帧点云,输出是两帧点云之间的变换矩阵,即寻找一个空间的最优旋转矩阵,让两个点云之间的距离最小. 算法步骤: ① 找出后一帧点云中的点在前一帧点云中的对应近点 ② 求一个使得上一 ...
干货 | 三维点云配准：ICP 算法原理及推导
编者荐语点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...
点云配准2：icp算法在PCL1.10.0上的实现+源码解析
目录本文最后实现的配准实例点云配准系列准备程序结构主程序 1.为什么要降采样 2.体素降采样原理 3.点云更新 icp 配准前的参数设置 icp配准算法内部对应点对确定(determine ...

迭代最近邻ICP算法

迭代最近邻ICP算法相关推荐

最新文章

热门文章