三维点云处理
Cluster
对给定点集(p_i, i in {1,2,...n})进行聚类。
K-means
(1) 随机初始化K个中心点
(2) 每个点(p_i)属于分配一个距离最近的点
(3) 更新中心点位置
(4) check converge
变种：
K-Medoids（K中心点法）：中心点必须从origin点集中选出；能够有效解决outlier把中心点拉得很远的情况。
GMM（gaussian mixture model）：(2)中hard分配中心点变为soft; (3)每个中心点维护的不仅是坐标，而是一个guassian model包含((mu, sum, pi))三个参数，其中(pi)为该中心GM为其它GM混合时的权重。
Spectral Clustering
谱聚类与K-means类方法最大区别在于由基于欧式距离的聚类转换为“基于连接”的聚类，算法步骤：
(1) 建立n个点的邻接矩阵(M in R^{n imes n})与Degree矩阵D（对角阵，每个元素为M矩阵每行的和）
(2) Laplacian矩阵(L = D - W)
(3) 计算L的k个最小特征值对应的特征向量(v_1, ..., v_k)，构成矩阵(V in R^{n imes k})
(4) 将每V的每行取出得(y_i in R^{k})进行K-means聚类出(C_1, C_2,..., C_k)
(5) 最后根据每个(y_i)的聚类结果确定每个(p_i)的聚类结果
pros:
(a) 利用Laplacian矩阵良好特性：特征值为0的个数即为联通子图个数。
(b) eigengap: 根据特征值的gap能够自动确定聚类数k。
(c) 基于连接的聚类方式能够很好解决比如两个圆环或者两个月牙嵌套的这种case，相对于基于欧式距离的方法。
cons：
当n较大时，对(L in R^{n imes n})进行特征值分解计算量还是挺大的。
Mean-Shift
算法步骤：
(1) 在点集中任意放下半径为r的circle
(2) 重复将circle移动到包含点质心位置直到该circle位置不调整
(3) 重复步骤(1)(2)，即放下新的circle，如果circle重叠，则保留包含点最多的circle，直到circle数量不再增加
(4) 计算每个(p_i)距离哪个circle最近，归于该cluster。
pros：自动计算聚类中心数量；
cons: 不能处理"两个嵌套环"
DBSCAN
非常NB与实用的聚类算法，基于连接。算法步骤：
(1) 在点集中选择unvisited (p_i)点半径为r的circle
(2) 如果circle包含点数大于m则点(p_i)为core point，否则为noise。将点(p_i)置为visited
(3) 对于core point点对应circle里的点递归进行(1)(2)步骤，将所有标记出的core point记为cluster C并移出点集。
(4) 对于剩下unvisited点重复(1)(2)(3)步，直到所有点变为visited。
pros：
自动计算聚类中心；基于连接；对于outliers(noise)鲁棒。
cons:
半径r是人为设定参数，对于密度集中或者分散需要对就设计不同的r。
补充知识：RANSAC: random sample consensus
用来除去outlier，超参数：
( au)：经验值，点到参考直线距离 ( au)则为内点 => 记录内点率及内点坐标
(N)：迭代次数，通过期望内点率，以及期望最终confidence两个先验值估算出
最终将内点率最高的那次sample出来的直线作为拟合直线，对应的点为内点。
补充知识：Hough直线
用来拟合点集中一条或者多条直线。基本原理：
直线可以用(y = kx + b) 来表示，图像空间中一个点在参考数空间k-b中成为一条以k, b为变量的直线，而参数空间中从标为(k, b)的一个点对应为图像空间中的一条直线。给定点集中所有点在参数空间对应了很多线直线，这些直线会近似相交到几个点簇中，这几个相交点在图像中空中即为我们要拟合直线。实际操作中k=0这条直线不好表示，所以有时会转换到参数坐标系(( heta), t)或者极坐标系((
ho), ( heta))中。
点云特征点
应用背景：ICP通常需要好的初始状态下(两帧点云的overlap要大)才能迭代出准确的R/T变换。通常使用3D feature matching的方法来获取良好的初始解。
2D图像haris特征点
特征点最直观：将radius为r的小patch在点p附近shift (u, v)，内容变化的剧烈程度。
(E(u, v) = sum(I(x+u, y+v) - I(x,y))^2 = [u, v] M [u, v]^T)
其中，上式通过一阶泰勒展开得到的(M = egin{bmatrix} I_x^2 & I_xI_y \ I_xIy & I_y^2end{bmatrix})
如果将M里面每个((I_x, I_y))都画出来，可以发现：
如果patch里是空白，则所有点坍缩在一团；
如果patch里面是一条线，则散点分布类似于椭圆；
如果patch里面是两条线，则散点分布类似于两个相交的椭圆状。
其实椭圆长轴分布的范围就是特征值(lambda)，而方向就是特征向量所在方向。
早期一系列方法针对M特征值(lambda)来判断点p是否是特征点，称为Response函数R，常用的是(R=min(lambda_1, lambda_2))
3D点云haris特征点
当点云有intensity值时，情况与上述基本相同，不过拓展到3D空间。唯一区别是无法直观算出((I_x, I_y))，因为二维情况可以直接通过上下pixel的梯度，但三维空间领域点不好确定。三维情况主要通过两步求解：
(1) 我们要求解的就是点p(x, y, z)附近点(x', y', z')云梯度的变化。
((x' - x, y - y', z - z') dot (I_x, I_y, I_z) = I' - I)
点p半径为r附近有n个点就有n这个方程，通常是超定方程，可以通过最小二乘来解。
(2) 由于我们知道点p在平面上，则点p的梯度变化也一定在平面上。所以可以将(I)通过点p的法向量(n)投影到平面上，来去除噪点。
3D点云ISS特征点
ISS(intrinsic shape signature)：点p与附近r范围内其它点的协方差距阵(M in R^{3 imes N})，M的三个特征值分别为(lambda_1), (lambda_2), (lambda_3)。如果点p处于平面，则(lambda_1 = lambda_2 > lambda_3)；如果点p处于直线，则(lambda_1 > lambda_2 = lambda_3)；所以如果点p为某个角点，则特征值各不相等(lambda_1 > lambda_2 > lambda_3)。通过该条件筛选出所有特征点，再通过以(lambda_3)为score进行NMS。
USIP
基于深度学习的无监督方法。出发点在于特征点不论如何旋转特征点应该始终是特征点。做法是通过将PC随机旋转之后过feature point det后出的点经过某个变换之后应该还能完全对齐，基于这个pretext任务构造Loss让feature point det提出有意义的特征点。
点云中的特征描述子
有了特征点如何获取特征点局部的描述子呢？也分传统方法(PFH, FPFH)与深度学习方法。其中传统方法又分为基于histogram与基于signature两类方法。
PFH
Point Feature Histogram，传统方法，基于大小为(B^3)的histogram。
(1) 建立LRF(local reference frame)：每个pair对可以根据两点连线(<p_1, p_2>)及以两点法向量(n_1, n_2)来建立以(p_1, p_2)为中心的两个坐标系，这个两个坐标系的信息可以用角坐标((alpha, phi, heta))来表示。
(2) 点p附近的k个点两两之间pair可以形成(k^2)个角坐标((alpha, phi, heta))，通过将这些点散落到每个维度B个grid的3D voxel空间，再把该空间flatten成vector即可得到该特征点的descriptor (D in R^{B^3})。
评论：很直观暴力的方法，不过LRF的建立过程很常规还是可以学习一下。
FPFH
Fast Point Feature Histogram，传统方法，基于大小为(3B)的histogram。
(1) SPFH (Simple Point Feature Histogram)，同上建立LRF，得到角坐标((alpha, phi, heta))每个维度分别进行大小为B的histogram再连接到一起。
(2) 将点p附近的k个点的SPFH根据距离加权平均到一起就到得FPFH。
SHOT
Signature of Histograms of Orientations
将特征点p附近半径为r球面空间划分成32个区域(2x2x8，8根径线)，每个扇区计算出其norm方向(n_v)，每个特征点的法线方向计为(n_u)。
(cos( heta) = n_u dot n_v)
将(cos( heta))离散化出11个bin，则整个SHOT descritor共表示为 11 x 32 = 352dim的feature。
3D Match
基于DL的方法。
(1) 使用RGBD相机构建3D场景
(2) 针对每个key point提取30x30x30的3D voxel patch。
(3) Positive：不同pose下的同一个keypoint。Negative: 3d空间中不同的patch。
(4) 使用pair wise的constrastive loss对3d siamese network进行训练。
说明：pair wise的constrastive loss考虑pos < neg小到一个绝对长度(gamma)，提升空间是使用相对大小比较的triplet loss。
缺点：3D voxel patch对于旋转不鲁棒。
Perfect Match
3D Match的改进版本，主要引入LRF来解决rotation问题。点p处的LRF建立过程：(z_p = n_p)，(n_p)为法向量；(x_p)为所有(<p, p_i>)投影到平面形成向量的加权平均。(y_p = z_p imes x_p)。loss function使用的是triplet loss。
PPFNet
Point Pair Feature
之所以和之前工作不一样是直接使用point来提3D feature，而不是先打成3D voxel patch。但是每个点如果直接使用(x, y, z)坐标除了位置之外没有什么信息含量的。本文使用每对pair的(psi = (left | d
ight |, angle(n_1, n2), angle(n_1, d), angle(n_2, d)))作为一个点，那个pair中有(k^2)个pair就有很多点统一送到pointNet中提取feature，与全局feature进行concat再使用N-tuple Loss进行训练。
PPF-FoldNet
同上，使用(psi) + point进行特征提取，只不是Pretext任务不再是对比学习，而是reconstruction，即通过encoder+decoder能否还原出原来的point cloud，使用champer loss来对比两个点云是否一致。
总结
点云配准
Registration：通常建立在已经有比较好的Initialization的情况下，使用Registration进行Refine，常见的Intialization的方式有：
(a) PointCloud的特征点提取+Matching+RANSAC （类似于2D的image matching）
(b) IMU或者GPS等其它传感器辅助
下面介绍一阶Registration方法ICP，与二阶方法NDT。
ICP
iterative closest point，总共分四步：
(1) preprocess: random sample, norm space sample
(2) data-association: nearest neighbor (kdtree/octree)，normal shooting。总之都是比较粗暴的找到source PC与target PC之间逐点的对应关系，并且把距离过大的pair移除掉。
(3) Cost function:
Point2point
(E(R, t)=min_{arg_(R,t)}=frac{1}{N}sum_{N}^{i}left | q_i-R p_i -t
ight |)
可以证明该式存在近似的闭式解：
(t = mu_q - R mu_p)
R = UV^T
其中: (mu)为对应PC的质心，而(U),(V)为去中心化后两个点云的内积矩阵的正交矩阵。
Point2Plane
将target point使用surface normal来变成一系列切线集合，再利用source point到平面的关系建立cost function。point2plane不同于point2point，没有直接的解析解，需要优化求解R/t，我们知道R是有正交矩阵的约束。一种办法是使用李代数转换到exp空间进行优化求解。这里采用第二种方案，即将R使用欧拉角表示为(R(alpha)), (R(eta)), (R(gamma))，再得用每次旋转量很小的先验近似(sin( heta)= heta), (cos( heta)=1)，最终解出(alpha, eta, gamma)即可，每个点都可以建立一个方程，转换为(Ax=b)的问题，可以使用SVD分解来解出变量。
(4) check converge
如果迭代过程R,t不怎么变了，就可以停止迭代了。
NDT
normal distribution transform，使用了二阶方法（牛顿法）进行更新R/t
(1) 概率化target PC空间
解target PC变成voxel grid，将点数大于m的grid中的点的分布拟合出一个gaussian distribution，点数小于m的grid进行removal。
(2) Cost Function
问题转换成为如何将source PC进行R/t变换之后在targe gaussian space中的似然概率最大，将问题转换成为MLE（maximum likelihood estimation），待估计参数为R/t。
该Cost function使用二阶泰勒展开再令关于x的了阶导数为0可得到每次优化的步长即为一阶梯度(g)与二阶海森矩阵(H^{-1})的乘积，但二阶方法需要解海森矩阵H比较麻烦，实际上可以利用R的(sin( heta)= heta), (cos( heta)=1)来简化计算，这里就不展开说明了。
说明：每次优化一步即可，原因：(a) 二阶优化(牛顿法)一步就很快了。(b) 有一些近似再里面，迭代次数多也不精确。
总结：二阶牛顿法其实就是把原本一阶的切线替换成了二次函数，直接找二次函数顶点作为下一步的(x_{i+1})
(3) 同ICP进行R/t的update
(4) 同ICP进行check converge
优点：速度快，对initialization稍微不敏感一点。缺点：target space需要概率化，二阶法优化复杂。
点云cls/seg
点云具有无序性导致使用DL处理比较麻烦，早期存在三类方法：
(a) 将整个空间离散成voxel空间，比如occupy map/TSDF/ESDF等，代表性方法是VoxelNet。缺点：很多空白栅格白学浪费资源。
(b) 使用360度的N个相机阵列将点云投影到对应多个2D Image，再将2D Image Concat到一起用2D Conv处理。缺点：相机阵列数N很关键。
(c) 直接对无序点云进行训练，需要克服点云的无充性，所以训练的时候就得做一系列各种顺序的数据增强，效果还一定好。但是衍生出来PointNet系列还是很经典的。
PointNet
可以做点云cls/det。本质上 = Shared MLP + MaxPooling。
Shared MLP: n x 3 => n x 1024，即使用MLP对每个点进行升维。
MaxPooling：n x 1024 pool成1 x 1024，从而将n这个维度消除，并且与n的顺序无关。这步是解决(c)的关键模块。
中间会夹杂各种transform，其实影响不是很大。
PointNet for seg: n x 1024 => n x m，其中m为类别数，从而达到对每个点进行分类的效果。
优点：速度快
PointNet++
相对于PointNet主要两个改进：
(1) 以set abstraction作为基本单位构成更深Network，set abstraction由以下三个部分构成：
sampling layer: 对点云进行采样，使用FPS(Furthest point sample)，具体做法是先随机选一个点，然后选择离这个点最远点作为第二点，再选离这两个点最远的点作为第三点，以此类推直到选够N个点。
grouping layer: 以采样出的点为中心，取出距离每个点一定范围radius之内的最近K个点为一个group。输出N x K x (d + C)，N为采样点个数，K为每个group最多点数，d是坐标维度，C是每个点属性特征。
pointnet layer: 就是上述的PointNet
(2) 上述grouping过程可以使用多种radius，每个radius提取出的K个采样点范围不同，从而起来类似于multi-scale的效果。这么做主要出发点是有些部分点密度高，有些部分密度低，使用相同radius聚合不合理。
(3) Hierachical如下图所示，主要将不同downsample scale的feature进行融合。
不同group的点云有密度可能不同，低密度的group需要增大scale，并且有些group需要多个scale融合充分提取特征。
效果：最终超越PointNet %4以上，当然速度也会相应变慢。
MV3D
早期做基于pointcloud及RGB image做3D object detection的工作。
bird view：pointcloud沿着height拆成若份，每份投影出一个view。
front view：投出depth map
RGB view: 对应的camera的出数据。
三种视图分别过backbone投出roi pooling之后的feature再进行融合。
总结：早期方法，titan X上0.36s，与实用还有一定差距。
VoxelNet
naive版本就是将pointcloud空间打成NxWxW的空间，使用VFE（voxel feature encoder）/PointNet提出整个voxel空间的feature tensor，最后再将feature tensor使用3D CNN出feature map，最终再类似于FRCNN的RPN一样出每个anchor的(x, y, z, l, w, h, theta)。
因为实际只有10%的voxel里面有point，所以将这10%挑选出来单独过VFE/PointNet得到每个voxel的特征再用index对应回去，再过后面的3D CNN，可以加快速度。
对比
(1) MV3D, VoxelNet是早期经典方法；
(2) PointPillars = Pillar Feature Net + SSD；即Pillar Feature Net提取3D点云特征图变换成CxHxW的2d image feature map，再使用2d SSD直接出3D boxes；
(3) PointRCNN = PointNet++提3Dfeature + 3D RPN + 3D Roi Pooling + 3D boxes;
(4) Frustum PointNet = 2d detector出2d boxes + 逐box的3d instance seg by PointNet => 3D boxes
(5) PointPainting = 2d seg投影到3d point进行类别染色 + PointRCNN/PointPillars等方法出框
总结：目前貌似还是PointRCNN这类two stage的方法效果更好一些。