【Sift+ICP點雲】基于Sift+ICP算法的點雲資料配準算法matlab仿真

1.軟體版本

matlab2017b

2.系統原理

算法的整體步驟如下：

（1）兩張影像的特征點提取與比對，如sift、surf等特征。

（2）利用特征點進行影像的相對定向，同時采用ransac對錯誤比對進行剔除，如采用Hartley的8點法或Nister的5點法相對定向。輸出為相對定向參數、定向精度。

（3）利用相對定向參數生成核線影像，并進行密集比對。進行前方交會時，左影像外方位元素直接取初始值，右影像取值根據相對定向結果進行修正(保持基線長度)，得到影像點雲。輸出為視差圖、物方坐标的影像點雲。

（4）影像點雲與lidar點雲的配準。使用ICP算法，模型采用相似變換模型，即對尺度縮放、旋轉和平移參數進行估計。之後利用相似變換參數對初始的影像外方位元素進行修正。最終輸出為配準後的影像外方位元素。

（5）效果與精度評定。

通過特征點的提取，可以将兩個圖像拼接， 獲得如下的效果：

最後，可以獲得如下的雲點圖：

       這個部分，理論就不介紹了，最後通過對配準之後的點雲資料計算RMS值來分析對應的性能。将提取的雲點圖和雷射掃描雲點圖進行配準，獲得如下的效果：

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