点云算法对比ICP、NDT，LM，Ceres-Solver，KDTree，SVM和ComplexYOLO、CenterPoint、PNP
1：ICP
ICP（Iterative Closest Point），即最近点迭代算法，是最为经典的数据配准算法。其特征在于，通过求取源点云和目标点云之间的对应点对，基于对应点对构造旋转平移矩阵，并利用所求矩阵，将源点云变换到目标点云的坐标系下，估计变换后源点云与目标点云的误差函数，若误差函数值大于阀值，则迭代进行上述运算直到满足给定的误差要求.
ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。
ICP算法是基于EM(Expectation-maximization
algorithm)思想的方法，采用交替迭代法优化得到最优值。即ICP分为两步迭代优化，优化点云匹配及优化运动估计。
点云匹配是将两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，就作为一对匹配点。
运动估计就是根据得到得两帧点云得匹配情况，构建最小二乘方程，求解。假设已知两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是求最小二乘方程的解，即以下方程的解R与t。
求解得到运动估计，ICP采用SVD分解求的运动估计得最优值。
除了使用逐步迭代法(最速/牛顿/LM等)求解，ICP算法采用SVD分解的方法，求得运动估计最优解。在时间占用上比使用四元数计算要大，但是比迭代法小很多，但是在精度上提高了很多。
https://blog.csdn.net/weixin_35695879/article/details/103125575
基本思想：
方法二：
针对点云匹配，
也可以找到对应的点集
利用lm算法，非线性优化6个参数，损失最小即可。。
||||||||||||||||||||||||||||||||||
方法三：
ICP简化算法，已知配对的坐标点：
https://blog.csdn.net/cangqiongxiaoye/article/details/123759326
from numpy import *
from math import sqrt
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
def rigid_transform_3D(A, B):
assert len(A) == len(B)
N = A.shape[0];
mu_A = mean(A, axis=0)
mu_B = mean(B, axis=0)
AA = A - tile(mu_A, (N, 1))
BB = B - tile(mu_B, (N, 1))
H = transpose(AA) * BB
U, S, Vt = linalg.svd(H)
R = Vt.T * U.T
if linalg.det(R) < 0:
print ("Reflection detected")
Vt[2, :] *= -1
R = Vt.T * U.T
t = -R * mu_A.T + mu_B.T
return R, t
if __name__=='__main__':
# get data: R T
R = mat(random.rand(3,3))
t = mat(random.rand(3,1))
print ('R',R)
print ('t',t)
U,S,Vt = linalg.svd(R)
R = U*Vt
print ('R= U*Vt',R)
if linalg.det(R) < 0:
Vt[2,:]*=-1
R = U*Vt
# get data: A B
n = 10
A = mat(random.rand(n,3))
B = R*A.T + tile(t,(1,n))
B = B.T
print ("raw points A")
print (A)
print ("")
print ("raw points B")
print (B)
# start : get R && T
ret_R, ret_t = rigid_transform_3D(A,B)
print ("get ret_R")
print (ret_R)
print ("")
print ("get ret_t")
print (ret_t)
# ret_R ret_t
A2 = (ret_R*A.T)+ tile(ret_t,(1,n))
A2 =A2.T
err = A2-B
err = multiply(err,err)
err = sum(err)
rmse = sqrt(err/n)
print ("points A2")
print (A2)
print ("")
print ("points B")
print (B)
print ("")
print (rmse)
fig = plt.figure()
ax=fig.add_subplot(111,projection='3d')
ax.scatter(A[:,0],A[:,1],A[:,2])
ax.scatter(B[:,0],B[:,1],B[:,2],s=100,marker='x')
ax.scatter(A2[:,0],A2[:,1],A2[:,2],s=100,marker= 'o')
plt.legend()
plt.show()
2：NDT
目前三维配准中用的较多的是ICP迭代算法，需要提供一个较好的初值，同时由于算法本身缺陷，最终迭代结果可能会陷入局部最优。NDT配准，这个配准算法耗时稳定，跟初值相关不大，初值误差大时，也能很好的纠正过来。
依据下面伪码流程可以看出，
该算法主要的思路就是将目标点云刻画成多个概率分布，然后通过位姿变换关系将待配准点云转换到目标点云坐标系下，计算转换后待配准点云的总概率，并将此概率的负值作为目标函数，通过高斯牛顿迭代法优化该目标函数以求获得负的最小概率值(即最大概率值)．高斯牛顿迭代公式见第23行，由梯度向量、海森矩阵和待求的位姿增量组成，因此最后整个NDT算法可以简单的归为求解梯度向量、求解海森矩阵和求解位姿增量三个主要过程．下面主要分析较复杂的梯度向量和海森矩阵的求解．
NDT转换为格子，格子不存储实际的点云数据，
利用协方差描述
3：KD-tree
Kd-Tree，即K-dimensional
tree，是一棵二叉树，树中存储的是一些K维数据。在一个K维数据集合上构建一棵Kd-Tree代表了对该K维数据集合构成的K维空间的一个划分，即树中的每个结点就对应了一个K维的超矩形区域（Hyperrectangle）。
在介绍Kd-tree的相关算法前，我们先回顾一下二叉查找树（Binary Search Tree）的相关概念和算法。
二叉查找树（Binary Search Tree，BST），是具有如下性质的二叉树（来自wiki）：
1）若它的左子树不为空，则左子树上所有结点的值均小于它的根结点的值；
2）若它的右子树不为空，则右子树上所有结点的值均大于它的根结点的值；
3）它的左、右子树也分别为二叉排序树；
KD树优化匹配过程
KD树原理
KD树是每个节点均为K维数值的二叉树，其上的每个节点代表一个超平面，该超平面垂直于当前划分维度的坐标轴，并在该维度上将空间一分为二。其构建过程是循环选取数据点的各个维度来作为切分维度，将当前维度的中值作为划分点，递归处理各子树，直到所有数据点挂载完毕。KD树的一些优化 切分维度选择的时候，一般优先选择方差大的维度开始切分。
选择中值时，对数据量较大的维度，不一定严格取中值，可以随机采样一定的数据，并取采样的中值作为划分点，加快划分过程。
利用kd-tree实现点云的k近邻搜索（当k为1是就是最近邻点）以及按照搜索点为中心的半径距离进行搜索。
1）k近邻搜索
#include 2）按照搜索半径进行搜索
#include 4：LM算法
什么是Levenberg-Marquardt算法？
它是使用最广泛的非线性最小二乘算法，中文为列文伯格-马夸尔特法。它是利用梯度求最大（小）值的算法，形象的说，属于“爬山”法的一种。它同时具有梯度法和牛顿法的优点。当λ很小时，步长等于牛顿法步长，当λ很大时，步长约等于梯度下降法的步长。
在LM算法中，每次迭代是寻找一个合适的阻尼因子λ，当λ很小时，算法就变成了GAuss-Newton法的最优步长计算式，λ很大时，蜕化为梯度下降法的最优步长计算式。
5:Ceres-Solver
linear_solver_type：
信赖域方法中求解线性方程组所使用的求解器类型，默认为DENSE_QR，
其他可选项如下：
DENSE_QR：QR分解，用于小规模最小二乘问题求解；
DENSE_NORMAL_CHOLESKY&SPARSE_NORMAL_CHOLESKY：Cholesky分解，用于具有稀疏性的大规模非线性最小二乘问题求解；
CGNR：使用共轭梯度法求解稀疏方程； DENSE_SCHUR&SPARSE_SCHUR：SCHUR分解，用于BA问题求解；
ITERATIVE_SCHUR：使用共轭梯度SCHUR求解BA问题；
//输入
double ext[7];
ext[0] = q.x();
ext[1] = q.y();
******;
Eigen::Map<Eigen::Quaterniond> m_q = Eigen::Map<Eigen::Quaterniond>(ext);
Eigen::Map<Eigen::Vector3d> m_t = Eigen::Map<Eigen::Vector3d>(ext + 4);
ceres::LocalParameterization * q_parameterization = new ceres::EigenQuaternionParameterization();
ceres::Problem problem;
problem.AddParameterBlock(ext, 4, q_parameterization);
problem.AddParameterBlock(ext + 4, 3);
//构建损失
for(auto val : pData) {
ceres::CostFunction *cost_function;
cost_function = external_cali::Create(val);
problem.AddResidualBlock(cost_function, NULL, ext, ext + 4);
}
//构建优化算法
ceres::Solver::Options options;
//使用 DENSE_SCHUR分解
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
//使用 LEVENBERG_MARQUARDT（LM）算法
options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
cout << summary.BriefReport() << endl;
#include 6：SVM
SVM的全称是Support VectorMachine，即支持向量机，
主要用于解决模式识别领域中的数据分类问题，属于有监督学习算法的一种。SVM要解决的问题可以用一个经典的二分类问题加以描述。红色和蓝色的二维数据点显然是可以被一条直线分开的，在模式识别领域称为线性可分问题。然而将两类数据点分开的直线显然不止一条。
根据支持向量的定义我们知道，支持向量到超平面的距离为 d，其他点到超平面的距离大于 d
得到最大间隔超平面的上下两个超平面：
我们需要最大化这个距离，所以就存在一些样本处于这两条线上，他们叫支持向量
对偶问题引出：
要求解这个式子。我们可以通过求解对偶问题得到最优解，这就是线性可分条件下支持向量机的对偶算法，这样做的优点在于：一者对偶问题往往更容易求解；二者可以自然的引入核函数，进而推广到非线性分类问题。
参考的SVM解读：
https://blog.csdn.net/xietingcandice/article/details/44157127
7：ComplexYOLO
8：CenterPoint
framework
CenterPoint也有三维检测器的独有的特点：
1、在三维检测中，主干网络需要学习目标的旋转不变性和等变性。为了让网络更好的捕获这个特征，作者在中心点预测分支和回归分支各添加了一个可变卷积。中心点预测分支学习旋转不变性，回归分支学习等变性。
2、考虑到网络输出的旋转不变性，作者选择了圆形池化区域，而不是CenterNet中的方形区域。具体说，就是在鸟瞰中，只有当某中心半径r内没有具有更高置信度的中心时，该对象才被视为正，作者将该方法称为Circular
NMS。Circular NMS与基于3D IoU的NMS具有一样的抑制效果，但速度更快。3、基于上述的设计，检测器依然没有达到完美的旋转不变性和等变性。作者因此构建了一个由输入点云的四个旋转、对称副本组成的简单集合，并将这一集合共同输入CenterPoint，每一个都产生一个热力图和回归结果，然后简单地将这些结果求均值。
9：Point Process
直通滤波器 体素滤波器 统计滤波器 条件滤波
半径滤波器 双边滤波 高斯滤波
均匀采样滤波 移动最小二乘法光滑滤波
基于权重局部优化投影 (WLOP) 简化算法 DoN算法
以及各种通过规则进行点调整或者删除的都可以成为滤波算法
直通滤波器：对于在空间分布有一定空间特征的点云数据，比如使用线结构光扫描的方式采集点云，沿z向分布较广，但x,y向的分布处于有限范围内。此时可使用直通滤波器，确定点云在x或y方向上的范围，可较快剪除离群点，达到第一步粗处理的目的。
体素滤波器：体素的概念类似于像素，使用AABB包围盒将点云数据体素化，一般体素越密集的地方信息越多，噪音点及离群点可通过体素网格去除。另一方面如果使用高分辨率相机等设备对点云进行采集，往往点云会较为密集。过多的点云数量会对后续分割工作带来困难。体素滤波器可以达到向下采样同时不破坏点云本身几何结构的功能。
统计滤波器：考虑到离群点的特征，则可以定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。给定均值与方差，可剔除3∑之外的点。
条件滤波：条件滤波器通过设定滤波条件进行滤波，有点分段函数的味道，当点云在一定范围则留下，不在则舍弃。
半径滤波器：半径滤波器与统计滤波器相比更加简单粗暴。以某点为中心画一个圆计算落在该圆中点的数量，当数量大于给定值时，则保留该点，数量小于给定值则剔除该点。此算法运行速度快，依序迭代留下的点一定是最密集的，但是圆的半径和圆内点的数目都需要人工指定。
10：PnP
PnP 问题有很多种求解方法：
用3对点估计位姿的 P3P、
直接线性变换(DLT)、
EPnP(Efficient PnP)、
UPnP等。
此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，即Bundle Adjustment。
https://blog.csdn.net/qq_37394634/article/details/104430656
PnP(Perspective-n-Point)是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时,如何估计相机所在的位姿。——《视觉SLAM十四讲》
通俗的讲，PnP问题就是在已知世界坐标系下N个空间点的真实坐标以及这些空间点在图像上的投影，如何计算相机所在的位姿。罗嗦一句：已知量是空间点的真实坐标和图像坐标，未知量（求解量）是相机的位姿。
PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P 、直接线性变换(DLT)、EPnP。此外,还能用非线性优化的方式,构建最小二乘问题并迭代求解,也就是万金油式的 Bundle Adjustment。下面介绍逐一介绍。
int main ( int argc, char** argv )
{
if ( argc != 5 )
{
cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
return 1;
}
//-- 读取图像
Mat img_1 = imread ( argv[1], CV_LOAD_IMAGE_COLOR );
Mat img_2 = imread ( argv[2], CV_LOAD_IMAGE_COLOR );
vector<KeyPoint> keypoints_1, keypoints_2;
vector<DMatch> matches;
find_feature_matches ( img_1, img_2, keypoints_1, keypoints_2, matches );
cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;
// 建立3D点
Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED ); // 深度图为16位无符号数，单通道图像
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
vector<Point3f> pts_3d;
vector<Point2f> pts_2d;
for ( DMatch m:matches )
{
ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
if ( d == 0 ) // bad depth
continue;
float dd = d/5000.0;
Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
}
cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;
Mat r, t;
solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
Mat R;
cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
cout<<"R="<<endl<<R<<endl;
cout<<"t="<<endl<<t<<endl;
//cout<<"calling bundle adjustment"<
bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}
