【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);
end

function [ 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