点云配准论文复现：Robust generalized point cloud registration with orientational data based on expectation ma

参考论文

Min Z, Wang J, Meng M Q H. Robust generalized point cloud registration with orientational data based on expectation maximization[J]. IEEE Transactions on Automation Science and Engineering, 2019, 17(1): 207-221.

思路

该论文提出一种杂交混合模型（hybird mixture model），使用高斯混合模型（GMM）和冯·米塞斯混合模型（von-Misis-Fisher model）分别表示点云位置和法向信息，进而对点云进行配准（registration）

复现

我使用matlab对论文进行了复现，全部代码如下：
主文件：demoHMM.m

%% load and generate data
datas = dlmread("./datas/bunnynormal.txt");
X = datas(:,1:3);
Xn = datas(:,4:6);
% rotate points and normal
angle2rotation = @(theta) [1 0 0;0 cos(theta) -sin(theta);0 sin(theta) cos(theta)] ...
                         *[cos(theta) 0 sin(theta);0 1 0;-sin(theta) 0 cos(theta)] ...
                         *[cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0; 0 0 1];
theta = pi/20;
rMatrix = angle2rotation(theta);
Y = transpose(rMatrix * transpose(X)) + repmat([1 1 1],size(X,1),1);
Yn = transpose(rMatrix * transpose(Xn));
% plot 
figure(1);
hold on;
scatter3(X(:,1),X(:,2),X(:,3));
scatter3(Y(:,1),Y(:,2),Y(:,3));
%% set initinal parameters
R = eye(3);
t = [0 0 0]';
k = 10; % von-Misis-Fisher模型参数
N = size(X,1);
M = size(Y,1);
sigma2 = 0;
for n = 1:N
    pX = X(n,:);
    for m =1:M
        pY = Y(m,:);
        sigma2 = sigma2+sum((pX-pY).^2);
    end
end
sigma2 = sigma2/(3*M*N);
%% call HMM function
[~,~,~,~]=HMM(X',Xn',Y',Yn',R,t,k,sigma2);

函数文件：HMM.m

function [Y,Yn,R,t] = HMM(X,Xn,Y,Yn,R,t,k,sigma2)
% HMM: utilizing Hybird Mixture Model for registration of orientational
% data
% REFERENCE:Robust Generalized Point Cloud Registration With Orientational Data Based on Expectation Maximization
% INPUT PARAMETERS:
% X: fixed point cloud. Xn = [x1 x2 x3...xn]
% Xn: normals of fixed point cloud
% Y: movable point cloud
% Yn: normals of movable point cloud
% R: initial rotation matrix of Y and Yn
% t: initial translation vector of Y and Yn
% k: initial parameter of Von-Misis-Fisher distribution
% sigma2_: initial parameter of Gaussain distrubution
% OUTPUT PARAMETERS:
% Y: transformed point cloud
% Yn: transformed point cloud normals
% R: final rotation matrix
% t: final translation matrix

w = 0.3;% weight of uniform distribution
iter = 5;
N = size(X,2);
M = size(Y,2);
for i = 1:iter
    %----------- E-step ----------%
    g = k/(2*pi*(exp(k)-exp(-k))*power(2*pi*sigma2,1.5));
    h = zeros(M,N);
    for m = 1:M
        for n = 1:N
            h(m,n) = k*transpose(R*Yn(:,m))*Xn(:,n)-sum((X(:,n)-(R*Y(:,m)+t)).^2)/(2*sigma2);
            h(m,n) = exp(h(m,n));
        end
    end
    % calculate pmn
    p = zeros(M,N);% posterior probability
    for n = 1:N
        sh = sum(h(:,n));
        for m = 1:M
            p(m,n) = (1-w)*(1/M)*g*h(m,n)/((1-w)*g*(1/M)*sh+w/N);
        end
    end
    %----------- M-step ----------%
    % M rigid step: calculate t and R
    Np = sum(sum(p));
    ux = [0 0 0]';
    uy = [0 0 0]';
    for n = 1:N
        for m = 1:M
            ux = ux + p(m,n)*X(:,n);
            uy = uy + p(m,n)*Y(:,m);
        end
    end
    ux = ux / Np;
    uy = uy / Np;
    t = ux - R*uy; % update t
    % update R: 1.construct xn' and ym' 2.construct H1 and H2
    % 3.SVD
    Xnew = X - repmat(ux,1,N);
    Ynew = Y - repmat(uy,1,M);
    H1 = zeros(3);
    H2 = zeros(3);
    for n = 1:N
        for m = 1:M
            H1 = H1 + p(m,n)*Ynew(:,m)*Xnew(:,n)'./sigma2;
            H2 = H2 + k*p(m,n)*Yn(:,m)*Xn(:,n)';
        end
    end
    H = H1+H2;
    [U,~,V] = svd(H);
    R = V*diag([1 1 det(V*U')])*U'; % update R
    % update point clouds
    Y = R*Y + t;
    Yn = R*Yn;
    figure(2);
    clf(2);
    hold on;
    text(0,0.25,num2str(i));
    scatter3(X(1,:),X(2,:),X(3,:));
    scatter3(Y(1,:),Y(2,:),Y(3,:));
    hold off;
    % M-var step
    sigma2 = 0;
    for n = 1:N
        for m = 1:M
            sigma2 = sigma2 + p(m,n)*sum((X(:,n)-(R*Y(:,m)+t)).^2);
        end
    end
    sigma2 = sigma2./(3*Np);
    % M-con step
    k_new = 0;
    for n = 1:N
        for m = 1:M
            k_new = p(m,n).*transpose(R*Yn(:,m))*Xn(:,n);
        end
    end
    k_new = (exp(k)+exp(-k))/(exp(k)-exp(-k))-k_new/Np ;
    k = 1/k_new;
end
end

实验效果

我使用bunny模型进行测试，在测试前需要计算点云法矢，代码运行效果如下图：

我使用均方根误差（RMSE）来对配准效果进行评价，误差随迭代变化关系如下图：

迭代过程中的误差具体数值为：

源代码下载地址：https://github.com/sulingjie/-registration