【Sift+ICP点云】基于Sift+ICP算法的点云数据配准算法matlab仿真
1.软件版本
matlab2017b
2.系统原理
算法的整体步骤如下：
（1）两张影像的特征点提取与匹配，如sift、surf等特征。
（2）利用特征点进行影像的相对定向，同时采用ransac对错误匹配进行剔除，如采用Hartley的8点法或Nister的5点法相对定向。输出为相对定向参数、定向精度。
（3）利用相对定向参数生成核线影像，并进行密集匹配。进行前方交会时，左影像外方位元素直接取初始值，右影像取值根据相对定向结果进行修正(保持基线长度)，得到影像点云。输出为视差图、物方坐标的影像点云。
（4）影像点云与lidar点云的配准。使用ICP算法，模型采用相似变换模型，即对尺度缩放、旋转和平移参数进行估计。之后利用相似变换参数对初始的影像外方位元素进行修正。最终输出为配准后的影像外方位元素。
（5）效果与精度评定。
通过特征点的提取，可以将两个图像拼接， 获得如下的效果：
最后，可以获得如下的云点图：
这个部分，理论就不介绍了，最后通过对配准之后的点云数据计算RMS值来分析对应的性能。将提取的云点图和激光扫描云点图进行配准，获得如下的效果：
RMS值为ICP迭代匹配的迭代过程。通过ICP算法，可以获得两个点云的最佳匹配效果。
function [TR,TT,ER,t] = func_ICP(q,p,NCP);
load parameter.mat
t = zeros(NCP+1,1);
Np = size(p,2);
%转换后的数据点云
pt = p;
ER = zeros(NCP+1,1);
T = zeros(3,1);
R = eye(3,3);
TT = zeros(3,1, NCP+1);
TR = repmat(eye(3,3), [1,1, NCP+1]);
%算法迭代
for k=1:NCP
switch arg.Matching
case 'bruteForce'
[match mindist] = func_match_bruteForce(q,pt);
case 'Delaunay'
[match mindist] = func_match_Delaunay(q,pt,DT);
case 'kDtree'
[match mindist] = func_match_kDtree(q,pt,kdOBJ);
end
if arg.EdgeRejection
p_idx = not(ismember(match,bdr));
q_idx = match(p_idx);
mindist = mindist(p_idx);
else
p_idx = true(1,Np);
q_idx = match;
end
if k == 1
ER(k) = sqrt(sum(mindist.^2)/length(mindist));
end
switch arg.Minimize
case 'point'
weights = arg.Weight(match);
[R,T] = func_eq_point(q(:,q_idx),pt(:,p_idx), weights(p_idx));
case 'plane'
weights = arg.Weight(match);
[R,T] = func_eq_plane(q(:,q_idx),pt(:,p_idx),arg.Normals(:,q_idx),weights(p_idx));
case 'lmaPoint'
[R,T] = func_eq_lmaPoint(q(:,q_idx),pt(:,p_idx));
end
%添加到总转换
TR(:,:,k+1) = R*TR(:,:,k);
TT(:,:,k+1) = R*TT(:,:,k)+T;
pt = TR(:,:,k+1)*p+repmat(TT(:,:,k+1),1,Np);
%目标函数的根的意思
ER(k+1) = func_rms(q(:,q_idx), pt(:,p_idx));
end
ER(1) = [];
if not(arg.ReturnAll)
TR = TR(:,:,end);
TT = TT(:,:,end);
endfunction [ imgout,X1,Y1,X2,Y2] = imMosaic( img1,img2,adjColor,STEP)
[matchLoc1,matchLoc2,X1,Y1,X2,Y2] = siftMatch(img1,img2,STEP);
[H,corrPtIdx] = findHomography(matchLoc2',matchLoc1');
tform = maketform('projective',H');
img21 = imtransform(img2,tform);
[M1,N1,dim] = size(img1);
[M2,N2,dim2] = size(img2);
if exist('adjColor','var') && adjColor == 1
radius = 2;
x1ctrl = matchLoc1(corrPtIdx,1);
y1ctrl = matchLoc1(corrPtIdx,2);
x2ctrl = matchLoc2(corrPtIdx,1);
y2ctrl = matchLoc2(corrPtIdx,2);
ctrlLen = length(corrPtIdx);
s1 = zeros(1,ctrlLen);
s2 = zeros(1,ctrlLen);
for color = 1:dim
for p = 1:ctrlLen
left = round(max(1,x1ctrl(p)-radius));
right = round(min(N1,left+radius+1));
up = round(max(1,y1ctrl(p)-radius));
down = round(min(M1,up+radius+1));
s1(p) = sum(sum(img1(up:down,left:right,color)));
end
for p = 1:ctrlLen
left = round(max(1,x2ctrl(p)-radius));
right = round(min(N2,left+radius+1));
up = round(max(1,y2ctrl(p)-radius));
down = round(min(M2,up+radius+1));
s2(p) = sum(sum(img2(up:down,left:right,color)));
end
sc = (radius*2+1)^2*ctrlLen;
adjcoef = polyfit(s1/sc,s2/sc,1);
img1(:,:,color) = img1(:,:,color)*adjcoef(1)+adjcoef(2);
end
end
pt = zeros(3,4);
pt(:,1) = H*[1;1;1];
pt(:,2) = H*[N2;1;1];
pt(:,3) = H*[N2;M2;1];
pt(:,4) = H*[1;M2;1];
x2 = pt(1,:)./pt(3,:);
y2 = pt(2,:)./pt(3,:);
up = round(min(y2));
Yoffset = 0;
if up <= 0
Yoffset =-up+1;
up = 1;
end
left = round(min(x2));
Xoffset = 0;
if left<=0
Xoffset = -left+1;
left = 1;
end
[M3,N3,dim3] = size(img21);
imgout(up:up+M3-1,left:left+N3-1,:) = img21;
imgout(Yoffset+1:Yoffset+M1,Xoffset+1:Xoffset+N1,:) = img1;
end
A09-49