taifyang的博客

算法的流程基本上和原理能对得上，但是实现过程中发现该算法结果不太稳定。可能实现有些问题吧，希望有懂的大神指出来（逃~）

但是笔者建议使用向量的方式，更为简单方便，三点在同一个平面上，以P1为基点，寻找两条向量P12、P13；在得到平面的法向量之后，带入其中一个点到方程(4)中即可得到平面约束方程。由初中的几何知识我们可以知道，确定一个三角形至少需要三个不共线的点，因此确定一个三角形的外接圆至少可用三个点。我们不妨假设三个点坐标为P1(x1,y1,z1),P2(x2,y2,z2),P3(x3,y3,z3)。一个空间圆的产生可以看作过该圆心的一个球体，被一个经过该点的平面所截而得到。

【代码】PointXYZRGB点云和meshcolor网格互相转化。

【代码】点云引导滤波。

【代码】PCL三维重建。

发现该算法在点云初始位姿较好时可以精调优化位姿（怎么感觉和LZ之前了解的ndt算法一般用于粗配准不太一样？下面的代码写的比较粗糙，上图原理基本上都实现了。不过只实现了第一次迭代，后续迭代过程LZ看了PCL中的ndt.hpp源码，需要使用高斯牛顿法求最小值，其中牵涉到了梯度下降方向，步长等问题（可见ComputeStepLengthMt函数），在源代码里也提到了论文名称，原理比较复杂就没有继续深究了。这个算法实现下来感觉还是有些复杂的，主要是公式推导比较多，论文中的求解过程还是比较清晰的。

其中是源点，是对应的目的点，是处的单位法线向量。该式没有解析解，我们只能使用非线性最小二乘的方法求解，为了加快求解，有研究者发现当两个输入表面之间的相对方向较小时，可以使用近似的方法来有效地求解的线性最小二乘法近似非线性优化问题。使用点到平面（point-plane）误差度量的迭代最近点 (ICP) 算法已被证明比使用点到点（point-point）误差度量的算法收敛得更快。在 ICP 算法的每次迭代中，产生最小点到平面误差的相对位姿变化通常使用标准的非线性最小二乘法来解决。多次迭代后直到算法收敛。

【2】基于KDTree改进的ICP算法在点云配准中的应用研究。在此基础上修改，直接读入pcd点云，加入了迭代求解。【1】改进的ICP算法在点云配准中的应用。代码参考：slam14讲高博的代码。

其中15-17行可以换成其他对点云的操作，这里是对点云进行沿x方向的平移。在while循环之前定义viewer，并通过addPointCloud方法添加点云；在循环中对点云进行操作，然后调用updatePointCloud刷新点云，调用spinOnce重新渲染输出，并调用延时函数防止刷新太快看不到显示效果。

【代码】normalspace_sampling法线空间采样实现。

【代码】farthest_sampling最远点采样实现。

LZ之前的文章[ICP算法实现（C++）](https://blog.csdn.net/taifyang/article/details/113898308) 用C++实现了基础的ICP算法，由于该算法是一种迭代的优化算法，里面含有大量循环操作以及矩阵运算，可以通过使用多线程或者GPU硬件来进行加速，具体分别可以通过OpenMP和CUDA编程实现。

外侧轮廓提取用的方法是经纬线扫描法，全轮廓提取的方法是alpha shapes算法（），从全轮廓中剔除外侧轮廓得到内侧轮廓。

建议使用方法一，代码实现更简单，而且速度较快（erase操作会使迭代器位置改变，影响效率）；除非要求对原始点云操作才用方法二。step2 遍历点云1中的点，记录下到点云2中的点的距离小于搜索半径的点的索引。基于kd-tree搜索的方法较快速，当然也可以暴力搜索。step3 方法一：将点云1中不在索引中的点保存下来作为结果点云3。step1 在点云2建立kd-tree，设置容忍误差（搜索半径）这里的求差集指的是从点云1中删除属于点云2的点。方法二：直接对点云1操作，删除索引点。

点云法线估计原理参考：[PCL]2 点云法向量计算NormalEstimation点云法向量估计原理及应用PCL点云曲率计算原理参考：二次曲面拟合求点云的曲率（曲率系列3：）PCL：PCL库中的两种曲率表示方法pcl::NormalEstimation和PrincipalCurvaturesEstimation代码实现：std::vector<double> FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::P

原理介绍：（1）首先计算出来各点的曲率值，将曲率值按照从小到大的顺序进行排序。（2）设置一空的种子点序列和一个空的聚类数组。（3）选取曲率最小的点放入上述种子点序列中。（4）从序列中拿出来一个种子点搜索其邻域。（5）搜索到邻域后，这些点先过法线夹角阈值，通过的保留到聚类数据，然后再从这些通过法线夹角阈值的点中，检查是否通过曲率阈值，通过的加入种子点序列。即比较邻域点的法线与当前种子点法线之间的夹角，如果小于阈值将该邻域点加入聚类数组。再从这些小于法线阈值的点中判断是否小于曲率阈值，如果小于则将该邻

代码实现：void euclideanclusters(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clusters, float threshold, int min_cluster_size, int max_cluster_size){ pcl::KdTreeFLANN<pcl::Point.

经纬线扫描法经纬线扫描法的基本思想是：将经过坐标变换后的二维数据点集按照 x值的大小排序后存储在一个链表中，在二维平面建立点集的最小包围盒并分别计算出 x 和 y 的最大、最小值；令经线从 x 的最小值开始，取步长为dx，在 x 的取值范围内分别计算出每根经线的最大和最小 y 值，并将它们的索引值放在一个新建的链表中，扫描完整个 x 区间；同样的方法扫描纬线，最后将重复的索引值删掉。#include <iostream>#include <algorithm>#include

大致思路：定义起始种子点后通过当前点k邻域搜索找其最邻近点，调整邻近点法线朝向使其与当前种子点法线朝向同向，然后传播出去。具体实现：（1）用vector模拟两个栈points和normals，存放点云的点和法线。首先将初始种子点压入这两个栈。（2）设置一个和点云点数相同大小的flags用来标记点云中的点是否被生长过，并将初始种子点的标志位置为true。（3）建立点云的kd树。（4）分别从points和normals弹出最后一个值作为当前种子点，计算该点对应的法线，并求出其k邻域点。（5）遍历该点的

原理：---------------基于ransac算法平面检测：1、确定迭代次数；2、在迭代次数内：2.1 随机选择三个点组成平面（判断三个点是否共线）；2.2 构造坐标矩阵；2.3 求平面方程；2.4 求所有点到平面的距离；2.5 统计inlier个数（距离小于阈值）；3、迭代选择inlier个数最多的平面。已知三个点坐标为P1(x1,y1,z1), P2(x2,y2,z2), P3(x3,y3,z3)，求过他们的平面方程：设方程为A(x - x1) + B(y - y1) + C(

采用C++实现ICP（Iterative Closest Point，迭代最近点）算法，借助了PCL库实现点云基本变换、KD-tree以及可视化部分，核心迭代部分没有调用PCL的api。代码在KD-tree搜索部分采用了openmp加速优化，在适当的数据集下运行速度能超过PCL自带的ICP算法配准api。ICP算法的原理和推导，网上有很多，在此不赘述。下面链接是我觉得讲的比较详细的，可以参考：三维点云配准 – ICP 算法原理及推导ICP算法公式推导和PCL源码解析另外，本文的代码实现过程还参考了：

原理：实现：void FitPlaneByLeastSquares(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud){ if (cloud->points.size() < 3) return; double meanX = 0, meanY = 0, meanZ = 0; double meanXX = 0, meanYY = 0, meanZZ = 0; double meanXY = 0, meanXZ = 0,

实现：以平面模型投影为例void projection(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_projected, pcl::ModelCoefficients::Ptr coefficients){ float a = coefficients->values[0], b = coefficients->value

原理：对整个输入迭代一次，对于每个点进行半径R邻域搜索，如果邻域点的个数低于某一阈值，则该点将被视为噪声点并被移除。实现：void radiusremoval(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, double radius, int min_pts){ pcl::KdTreeFLANN<pcl::

原理：对每一点的邻域进行统计分析，基于点到所有邻近点的距离分布特征，过滤掉一些不满足要求的离群点。该算法对整个输入进行两次迭代：在第一次迭代中，它将计算每个点到最近k个近邻点的平均距离，得到的结果符合高斯分布。接下来，计算所有这些距离的平均值 μ 和标准差 σ 以确定距离阈值 thresh_d ，且 thresh_d = μ + k·σ。 k为标准差乘数。在下一次迭代中，如果这些点的平均邻域距离分别低于或高于该阈值，则这些点将被分类为内点或离群点。实现：float distavg(vector<

原理实现void voxelgrid(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered, float leafsize){ pcl::PointXYZ minPt, maxPt; pcl::getMinMax3D(*cloud, minPt, maxPt); float lx = maxPt.x - minPt.