点云配准（四） Sparse Point Registration 算法浅析
Sparse Point Registration （SPR）是一篇2017年的点云配准算法，该算法的主要目的是对稀疏点云进行配准，并且取得了不错的成果和突破。本文一方面是对SPR配准算法模型进行了简单的原理解析以及附加代码实现，另一方面是对之前工作的总结，也算水篇博文，接下来的工作主要就是分割和光流预测方面的学习了。
一.算法模型概述
1.算法背景
所谓稀疏点云就是点数稀少的点云模型，有时我们需要用到一些物体上的关键点来和目标模型进行配准，计算一些关键指标。而传统的点云配准算法要求待配准的两片点云数量级相当，并且还包括粗配准和精配准两个阶段。经实验可得，传统点云配准算法在稀疏点云配准上表现较差，因此稀疏点云配准十分关键。
2.算法模型
SPR算法不需要进行粗配准就可以实现效果较好的稀疏点云配准效果，该算法的核心思想主要包括扰动、迭代、细化三个部分。SPR算法模型包括以下步骤：
- 初始化目标模型点云数据A，稀疏点云数据B，最大迭代次数MaxIterations，配准误差阈值Threshold、扰动次数P
- 根据点云Size和高斯分布计算当前迭代的扰动量（扰动量随着迭代次数不断减小，最后减小到零）获取P个扰动扰动变换矩阵，然后对当前B点云进行扰动变换
- 分别计算P个扰动变换后的稀疏点云B和目标模型点云A的 Cost （这里可以使用K-D Tree 计算最近点偏差和作为Cost），选择Cost最小的作为局部最优扰动
- 基于当前的最优扰动变换后的稀疏点云B，进行 ICP or DQF 算法精配准细化（本文选择ICP），获得精配准矩阵Tk，并计算此时的误差
- 若计算的误差小于最小误差e，则更新全局最优边换矩阵T和最小误差e为当前迭代结果。
- 重复迭代以上过程，直到达到最大迭代次数MaxIterations或计算误差e<配准误差阈值Threshold
3.算法流程图
除此之外，论文中还阐释了一些比较细节的注意事项和参数设置，比如最大迭代次数MaxIterations、配准误差阈值Threshold、扰动次数P设置为多少，如何进行扰动计算，以及如何在试验中取点等等，当然稀疏点云中越具有特征性的关键点配准效果越好。
二.算法实现（ICP版本）
本文的SPR算法实现使用Matlab编程进行，采用ICP的精配准版本，在实现过程中对原算法进行了改进：在迭代过程中，对每一个出现的扰动变换都进行ICP精配准计算，然后基于这个误差进行比较，而不只是原算法所言的选出最优扰动后再计算ICP，这样可以最大程度的提高算法精确度，但是牺牲了时间复杂度。下面给出部分代码。
（1）SPR.m
% SPR Main logic function:transform A sparse point cloud into B model point cloud coordinate system for registration
% - Params：
% - A:sparse point cloud [n,3]
% - B:model point cloud [m,3]
% - Return：
% - R:optimal rotation matrix [3,3]
% - T:optimal translation matrix [1,3]
% - trans_res:transform point cloud [n,3]
function [ R,T,trans_res ] = SPR(A,B)
%1.kdtree model for B(euclidean distance) and size of the model
Mdl = KDTreeSearcher(B);
Size = max(range(B));
%2.Initialize R0,T0,e0
R = eye(3);
%Align the centroids of two point clouds A->B
cen_A = sum(A)/length(A);
cen_B = sum(B)/length(B);
T = cen_B - cen_A;
trans_res = transform(A,R,T);
e = calculateCost(trans_res,B,Mdl);
%3.Initialize the number of disturbed pose P, the maximum number of iterations I, and the minimum error threshold RMS
P = 10;
I = 30;
RMS = 1e-6;
%4.Initialize loop variable
k = 1;
%record data
x_loop = 1:I;
y_error = zeros(1,I);
%5.SPR Loop
while e > RMS && k <= I
cost_best = inf;
R_k = R;
T_k = T;
%(1). Perturbations for Sparse point cloud,and then the perturbation transformation matrix is obtained
% - R_p: [3,3,P+1]
% - T_p: [p+1,3]
[R_p, T_p] = perturb(Size,P,k,I);
%(2). Calculate the optimal pose
for j = 1:P+1
%Obtain the transformation matrix parameters rp_j(3,3), tp_j(1,3) of the current p-th disturbance pose
rp_j = R_p(:,:,j)*R;
tp_j = (R_p(:,:,j)*T')'+T_p(j,:);
%Calculate current optimal cost(from ICP)
[R_j,T_j,cost_j] = solveicp(A,B,Mdl,rp_j,tp_j);
%judge optimal cost
if cost_j < cost_best
cost_best = cost_j;
R_k = R_j;
T_k = T_j;
end
end
%(3). Error calculation and judge for update
trans_k = transform(A,R_k,T_k);
e_k = calculateCost(trans_k,B,Mdl);
if e_k < e
e = e_k;
R = R_k;
T = T_k;
end
y_error(k) = e;
k = k+1;
end
trans_res = transform(A,R,T);
figure
plot(x_loop,y_error,marker='o', MarkerFaceColor='r');
xlabel('loop');
ylabel('ems error');
end（2）Perturb.m
%Disturbance function: Generates random perturbations based on iteration within loop and specified
% - Params：
% - size:Size of the model
% - P：Number of disturbances
% - k：Number of iterations
% - I：Iteration upper bound
% - Return：
% - R_p:Perturbation rotation matrix [3,3,P+1]
% - T_p:Perturbation translation matrix [P+1,3]
function [ R_p, T_p ] = perturb( size,P,k,I )
%1.Initial translation disturbance:10% of the model size
T_p = normrnd(0,(0.1*size*((I+1-k)/I)),[P,3]); %[P,3]
%2.Initial perturbation number of rotation:10 ° normal distribution(Right hand coordinate system)
%Around x
a = normrnd(0,(10*((I+1-k)/I)),[1,P]); %[1,P]
%Around y
b = normrnd(0,(10*((I+1-k)/I)),[1,P]);
%Around z
c = normrnd(0,(10*((I+1-k)/I)),[1,P]);
%3.convert to radians
a = a.*(pi()/180);
b = b.*(pi()/180);
c = c.*(pi()/180);
%4.initialize R_p
R_p = zeros(3,3,P);
%5.Generate P disturbances respectively (matlab subscript starts from 1)
for j = 1:P
%rotation matrix about x
r1 = [1,0,0; 0, cos(a(j)), -sin(a(j)); 0, sin(a(j)), cos(a(j))];
%rotation matrix about y
r2 = [cos(b(j)), 0, sin(b(j)); 0,1,0; -sin(b(j)), 0, cos(b(j))];
%rotation matrix about z
r3 = [cos(c(j)), -sin(c(j)), 0; sin(c(j)), cos(c(j)), 0; 0,0,1];
%Perturbation Rotation
R_p(:,:,j) = r1*r2*r3;
end
%P+1:Transformation without disturbance
R_p(:,:,P+1) = eye(3);
T_p(P+1,:) = [0,0,0];
end
（3）solveicp.m
%Function takes in A and B clusters, registers A to B using ICP, returns resultant transformation matrix and error calculation
% - Params：
% - A:initial point cloud [n,3]
% - B:target point cloud [m,3]
% - Return：
% - TR:solve rotation matrix [1,3]
% - RO:solve translation matrix [3,3]
% - e:registers error
function [ R,T,e ] = solveicp( A,B,Mdl,R_0,T_0 )
%1.initialize variable parameters
k = 1;
error = 0.0001;
Iterator = 50;
R = R_0;
T = T_0;
%2.initial:kdtree algorithm to find the nearest point pair of two point clouds
trans = transform(A,R,T);
e = calculateCost(trans,B,Mdl);
%3.ICP loop
while e > error && k <= Iterator
%find the nearest point
idx = knnsearch(Mdl,trans);
NN = B(idx,:);
%svd solve: optimal matching matrix of two point clouds in current state
[T_k, R_k] = solvesvd(trans,NN);
%Cumulative transformation T=[1,3] R=[3,3]
T = (R_k*T')'+T_k;
R = R_k*R;
trans = transform(A,R,T);
%Update error E
e = sum(sqrt(sum((NN-trans).^2,2)))/length(trans);
k = k+1;
end
end
（4）solvesvd.m
%function: svd solve yo find T and R between point set B and it nearest neighbour set
% - Params：
% - B:initial point clouds [n,3]
% - NN: target nearest neighbour point clouds [n,3]
% - Return：
% - T:solve optimal translation matrix [1,3]
% - R:solve optimal rotation matrix [3,3]
function [ T, R ] = solvesvd( B, NN )
num = length(B);
%1.Seeking centroid
cen_B = sum(B)/num;
cen_NN = sum(NN)/num;
%2.Decentralization:Set centroids as reference origin for both clusters
B2 = B-repmat(cen_B,num,1);
NN2 = NN-repmat(cen_NN,num,1);
%3.svd decompose:[U,S,V] = svd(A) ==> A = U*S*V'
H = B2'*NN2;
[U,~,V] = svd(H);
R = V*diag([1 1 sign(det(V*U'))])*U';
T = cen_NN' - R*cen_B';
T = T';
end
三.实现效果
注意：上图中蓝色为初始稀疏点云，黑色为配准后的稀疏点云结果
