ICP算法、Robust_ICP算法

关于点云配准算法的一些总结

点云配准算法ICP，以及稳健回归的ICP算法

本科毕业设计做的点云配准相关方面的工作，为此把做的内容做以下总结。

控制点选择方法

点云采样，是从整体的点云数据中按照一定的规则选取部分点云数据的过程。进行点云配准时，需要选择控制点，理论上要求至少需要三组不共线的对应点进行配准。为提高配准精度，选择特征明显的控制点进行配准，采样是一种比较高效的方法。

1. 随机采样 ，即按照随机原则，等概论抽取点云数据，抽样的结果具有偶然性，结果分布不均匀呈现杂乱状,如图3-1b。因此，随机采样会引入许多确定的因素，影响配准实验精度，为此进行配准时可能会陷入局部收敛。
2. 均匀采样：是按照一定的距离进行采样，采样点的空间分布具有规则性，分布较均匀如图3-1c。均匀采样得到的样本点云数据排列整齐。采样间隔越小，获取的样本越大。当采样间距比较大时，样本特征可能不明显，具有容错性，即包括错误或者存在误差的点云数据，而且这种误差不易被发现。
3. 八叉树采样是一种应用广泛的一种树状数据结构。八叉树采样是通过划分三维空间，第一次将原三维空间划分为8个小空间称为子节点，进行存储数据，子节点继续划分空间，直到达到最大递归深度递归的深度越大，采集到的样本越大，样本中包含的特征越多，但计算时间也会随之增加如图3-1d。
4. 基于曲率特征采样 它是评价曲面平坦程度的几何参数，较平坦的地方曲率小，较陡峭的地方曲率大。法曲率是描述点云曲面起伏情况的重要参数如图3-1f。

经典 ICP算法

能量函数主要包括点到点、点到线、点到面、线到线、线到面、以及面到面等的能量函数。不同的能量函数带来的收敛效果是不同的。

---

能量函数+查找对应点的方法**
kd树、Delaunay、迭代欧氏距离

稳健回归方法Robust IC

鲁棒配准方法，能在不确定的噪声干扰下实现精确配准。采用M估计来限制异常值的影响，可以视为迭代最近点配准算法的改进版本，使用迭代重新加权的最小残差平方和来合并鲁棒性，并到达较好的配准效果。采用Huber、Biweight、Cauchy、Welsch核函数进行定权。

算法1：鲁棒配准
输入：点云P=〖{P〗_i |P_i∈R_3,i=1,2,3…}，点云Q=〖{Q〗_i |Q_j∈R_3,j=1,2,3…}；设置最大迭代次数maxIter，停止迭代阈值thres；DT为三角化点云P的结果；w为点云间距离resid对应的权重；距离阈值Kresid。
流程：
(1).初始化变量：w矩阵（1×n矩阵）；T平移矩阵(3×1矩阵)；R旋转矩阵(3×3矩阵)；
For i=1:maxIter
(2).将点云P三角化得到DT，查找DT与Q的距离resid及其标签vi；
(3).设置参数k，筛选出resid较大的点，定义距离阈值Kresid=kmean(resid)；
(4).switch：
Case1：Huber定权法则Kresid= 2.0138Kresid;
Case2：Bi-weight定权法则Kresid=7.0589Kresid;
Case3：Cauchy定权法则Kresid=4.3040Kresid；
Case4：welsh定权法则Kresid=4.7536*Kresid；
(5).刚体变换：计算w ̂，p ̅（p ̅是3×1矩阵），y ̅（y ̅是3×1矩阵），计算H，SVD(H)，计算T，R。
(6).对PQ进行刚体变换，更新T，R。
(7).计算E^k判断是否小于thres，或者达到最大迭代次数maxIter
END
输出：配准结果

采样代码

*随机采样
%% 控制点的选择
[m,n]=size(data2);
control_num=200;
idx=randperm(m);
idx=idx(1:control_num);
control_data=data1(idx,:);
**均匀采样和八叉树采样类似（略去）
**
曲率特征采样
% 曲率特征
P=data’;
k=8;
[pn,pw] = pca(P, k);
pp = P+3.pn;%pn为法向量，pw为评价曲率的一个参数
%求取曲率最大的500个点
P_=P’;%P_ 是2500x3
[pw_,indice]=sort(pw);
indice=indice’;
%找出曲率最大（或最小）的几个点，
pw_first=indice(size(indice,1)-500000:1000:size(indice,1)😅;%曲率最大值排前1000的点
%pw_first=indice(1:1:1000,:);%曲率最小值（最平坦）排前1000的点+698/
for i=1:size(pw_first)
Point_pw(i,1)=P_(pw_first(i),1);
Point_pw(i,2)=P_(pw_first(i),2);
Point_pw(i,3)=P_(pw_first(i),3);%Point_pw为曲率的前500个点
end
关于PCA函数
% PCA主元分析法求法向量
% 输入：
% p:3n的数值矩阵
% k:k近邻参数
% neighbors = transpose(knnsearch(transpose§, transpose§, ‘k’, k+1));
% neighbors一般可缺省。若之前做过k邻域求取操作也可直接调用，提高运算效率
% 输出
% n:法矢，已规定方向由邻域拟合出的平面指向查询点
% w:用于评估曲率的参数，详见：Mark P,et al. Multi-scale Feature Extraction on Point-Sampled Surfaces[J]. Computer Graphics Forum, 2010, 22(3): 281-289.
function [n,w] = pca(p, k, neighbors)%p为输入的点云
if nargin < 2
error(‘no bandwidth specified’)
end
if nargin < 3
neighbors = transpose(knnsearch(transpose§, transpose§, ‘k’, k+1));%neighbor为一个索引矩阵，第一行代表第几个点，后8行代表K近邻的点。记录每个点及其周围的8个点
end
m = size(p,2);%返回第2维的维度
n = zeros(3,m); %存放法线的矩阵
w = zeros(1,m);
for i = 1:m
x = p(:,neighbors(2:end, i));%x为8个邻域点 ，3x8的矩阵
p_bar=mean(x,2);%每一行求均值(一共三行)

% P = (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k));%中心化样本矩阵，再计算协方差矩阵
% P = 1/(k) * (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k)); %邻域协方差矩阵P
P=(x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k))./(size(x,2)-1);
[V,D] = eig§;%求P的特征值、特征向量。 D是对应的特征值对角矩阵，V是特征向量(因为协方差矩阵为实对称矩阵，故特征向量为单位正交向量)
[d0, idx] = min(diag(D)); %d0为最小特征值 idx为特征值的列数索引。diag():创建对角矩阵或获取矩阵的对角元素
n(:,i) = V(:,idx); % 最小特征值对应的特征向量为法矢，即法向量
%规定法矢方向指向
flag = p(:,i) - p_bar;%由近邻点的平均点指向对应点的向量
if dot(n(:,i),flag)<0%如果这个向量与法向量的数量积为负数（反向）
n(:,i)=-n(:,i);%法向量取反向
end
if nargout > 1
w(1,i)=abs(d0)./sum(abs(diag(D)));%最小特征值的绝对值在协方差矩阵特征值绝对值的总和中占的比重
end
end