基于ICP算法的三维点云数据拼接算法的实现

Posted fpga&matlab

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了基于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.仿真结论:

D-72

 

以上是关于基于ICP算法的三维点云数据拼接算法的实现的主要内容,如果未能解决你的问题,请参考以下文章

基于ICP算法的三维点云模型的自动配准算法matlab仿真

点云数据处理

MATLAB教程案例49三维点云数据ICP配准算法的matlab仿真学习

PCL系列——三维重构之贪婪三角投影算法

算法:基于opengl 点云模型不可见点(隐藏点)的移除 最后一步搞不定了 分不是问题

PCL系列——三维重构之移动立方体算法