基于ICP算法的三維點雲資料拼接算法的實作

1.問題描述：

通過合并比例因子引入标準疊代最近點（ICP）算法中，我将配準問題轉化為一個7D的限制優化問題非線性空間。然後，我們應用奇異值分解（SVD）一種疊代求解此類優化問題的方法。最後，建立了一種新的ICP配準算法，稱為Scale-ICP算法具有各向同性拉伸的資料集。為了實作算法的全局收斂性，我們提出了一種初始注冊的選擇方法。證明所提出的方法的性能和效率算法中，我們給出了幾個與Scal算法的比較實驗

2.部分程式：

function c = Solvecircle(s,R,T,I,X,Y,Yo)

pointx = length(X(1,:));

%pointy = length(Y(1,:));

%X進行變化X--Xo--接近Y

Xo = s*R*X+repmat(T,[1 pointx]);

%dsearchn求Z,即Y中對應X的資料

k = dsearchn(Y',Yo,Xo');

Z = Y(:,k);

%計算目前ek內插補點

en = computeE(s,R,T,X,Z);

%計算H矩陣

xc = mean(X,2); %xc,zc為坐标中點

zc = mean(Z,2);

%H = zeros(3,3);

% 計算Xi,Zi

Xi = X - repmat(xc,[1 pointx]);

Zi = Z - repmat(zc,[1 pointx]);

%計算Rk+1

Rn = computeR(Xi,Zi);

%計算s-k+1 

sn = computeS(I,Rn,Xi,Zi);

%計算Tk+1

Tn = computeT(zc,sn,Rn,xc);

%對于k+1資料的e，fn

fn = computeE(sn,Rn,Tn,X,Z);

c = cell({Rn;Tn;sn;en;fn});

end

%計算Rk+1

function Rn=computeR(Xi,Zi)

H = Xi*(Zi');

[U S V] = svd(H);

if round(det(V*U')) == 1

    Rn = V*U';

elseif round(det(V*U')) == -1

    x = [1 0 0;0 1 0;0 0 -1];

    Rn = V*x*U';

end

end

%計算sk+1

function sn = computeS(I,Rn,Xi,Zi)

sn = sum(dot(Rn*Xi,Zi))/sum(dot(Xi,Xi));

if sn <=  I(1)

    sn = I(1);

elseif sn >= I(2)

    sn = I(2);    

end

end

%計算Tk+1

function Tn = computeT(zc,sn,Rn,xc)

Tn = zc - sn*Rn*xc;

end

%計算Ek+1

function e = computeE(s,R,T,X,Z) 

pointx = length(X(1,:));

c = s.*(R*X)+repmat(T,[1 pointx])-Z;

e = sum(dot(c,c));

end

3.仿真結論：