Frenet标架及曲率挠率计算

此文算法基于马国庆等论文“点云空间曲线的微分信息计算及匹配方法”，引用请注明出处，在此感谢

###Code

function [curveture,fT,fN,fB]=Frenet_Curveture(pt,k)
%计算连续曲线点的frenet标架及点的曲率
%参考：点云空间曲线的微分信息计算及匹配方法
%pt：三维点集
%k：领域大小
ptNum=size(pt,1);
curveture=zeros(ptNum,1);
hs=floor(k/2);
fT=zeros(ptNum,3);
fN=zeros(ptNum,3);
fB=zeros(ptNum,3);
for i=hs+1:ptNum-hs 
    Rx=pt(i,:);
    pj=pt(i-hs:i+hs,:);

    % frenet 标架计算
    centerDis = sum((pj-repmat(Rx,k,1)).^2,2);
    scenterDis=sort(centerDis);
    h=scenterDis((max(floor(k/3),1)+1));
    thetaj=exp(-centerDis/h);
    L=sum((pj-repmat(Rx,k,1)).*repmat(thetaj,1,size(pj,2)),1)/sum(thetaj);

    prl=pj'-repmat(Rx',1,k)-repmat(L',1,k);
    A=prl.*repmat(thetaj',size(pj,2),1)*prl';
    [vector,ldv]=eig(A);
    lgda=diag(ldv);
    [~,dex]=sort(lgda,'descend');
    t=vector(:,dex(1));
    n=vector(:,dex(2));
    b=vector(:,dex(3));
    t=t/norm(t);
    n=n/norm(n);
    b=b/norm(b);

    fT(i,:)=t;
    fN(i,:)=n;
    fB(i,:)=b;
    %曲线垂足计算
    alfa=L*n;
    beta=L*b;
    OX=Rx+alfa*n'+beta*b';
    % 曲率和挠率估计
    xj=(pj-repmat(OX,k,1))*t;
    yj=(pj-repmat(OX,k,1))*n;
    zj=(pj-repmat(OX,k,1))*b;

    wdis=sum((pj-repmat(OX,k,1)).^2,2);
    swdis=sort(wdis);           
    wh=swdis(max(1,floor(k/3))+1);
    wj=exp(-wdis/wh);

    K=2*sum(xj.^2.*yj.*wj)/sum(xj.^4.*wj);
    T=6*sum(zj.*xj.^3)/(K*sum(xj.^6));
    curveture(i)=abs(K);
end