python 最近邻距离 最近邻匹配算法

一、含义：

1.icp算法能够使两个不同坐标系下的点集匹配到一个坐标系中，这个过程就是配准，配准的操作就是找到从坐标系1变换到坐标系2的刚性变换。

2.icp的本质就是配准，但有不同的配准方案，icp算法本质是基于最小二乘的最优配准方法。该方法重复进行选择对应关系对，计算最优刚体变换，直到满足正确配准的收敛精度要求。

3.icp算法的目的就是找到待匹配点云数据与参考点云数据之间的旋转参数R和平移参数T,使得两点数据之间满足某种度量准则下的最优匹配。

4.每次操作的都是目标点集，使目标点集不断靠近参考点集，因此求R和T也是每次考虑目标点集中每个点在参考点集中的最近点；

二、流程图：

假设给两个三维点集x1和x2,ICP方法的配准方案步骤如下[1]：

第一步，计算x2中每一个点在x1点集中的最近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对x2使用上一步求得的平移参数和旋转参数，得到新点集；

第四步，如果新的变换点集和参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的x2继续迭代，直到达到目标函数的要求。

三、注意事项：

什么是目标点集，什么是参考点集？

目标点集就是你每次不断操作的点集，每次不断移动，使其不断靠近参考点集，每次求R和T时是在参考点集中查找和目标点集最近的点，操作的对象目标点集。

四、程序代码:

在网上参考了很多版本，都不太适合新手入门，本人写了一个比较简单的代码，希望与大家共同交流学习：

clc;
clear all;
clear all;
%产生点集1
x1=1:0.01:2;
sample=randn(1,length(x1));
y1=10*x1+2*sample;
pointcloud1=[x1;y1];
theta=pi/3;
R=[cos(theta) -sin(theta);
       sin(theta) cos(theta) ];
T =[4;5] *ones(1,length(pointcloud1(1,:))) ;
pointcloud2=R*pointcloud1+T;
sourch_point=pointcloud1;%参考点云
desti_point=pointcloud2;%目标点云
error=1;%初始误差
nit=0;%计数
Iter=20;%最大迭代次数
%目标点云不断向参考点云靠近
%可认为初始R为eyes(2);
%和初始平动t=[0;0];
%没有任何平动或者移动
while error>0.01
	index=[];
	nit=nit+1;
	if nit>iter
		break;
	end
	for i=1:length(desti_point(1,:))%为每一个目标点云中的点寻找对应点
		dx=(source_point-repmat(desti_point(:,i),1,length(source_point(1,:))));
		dist=sqrt(dx(1,:).^2+dx(2,:).^2);
		[dist,ii]=min(dist);
		index=[index;;ii];
		error=error+dist
	end
	source_point_part=source_point(:,index);
	%求质心
	u_sour_point=(sum(source_point_part,2))/length(source_point_part(1,:));
	u_dest_point=(sum(desti_point,2))/length(source_point_part(1,:));
	%去中心化
	source_point_center=source_point_part-u_sour_point*ones(1,length(source_point_part(1,:)));
	desti_point_center=desti_point-u_dest_point*ones(1,length(desti_point(1,:)));
	%求W
	W=source_point_center*desti_point_center';
	%SVD分解
	[U,S,V]=svd(W);
	%求R和t
	R=U*V';
	t=u_sour_point-R*u_dest_point;
	%变换后的点集
	desti_point=R*desti_point+t*ones(1,length(desti_point(1,:)));
	%画图观察变换后的目标点集
	figure;
	plot(pointcloud1(1,:),pointcloud1(2,:),'r.');
	hold on;
	plot(pointcloud2(1,:),pointcloud2(2,:),'b.');
	hold on;
	plot(desti_point(1,:),desti_point(2,:),'y.');
	error=1/length(desti_point(1,:))*sum(sum(((source_point_part-desti_point).^2),2),1);
end

五、程序运行结果：

结果图：

红色为参考点云，蓝色为目标点云，黄色为最终经过不断旋转和移动的目标点云