【C++】相机标定源码笔记-点云处理与分析的功能类

类主要涉及点云数据的处理、平面拟合和刚体变换矩阵的计算。通过这些函数可以实现对点云数据的处理和分析，包括坐标变换、平面拟合和旋转矩阵的计算。

点对应关系结构体，用于保存两个点云之间的对应点信息： 查询（源）点的索引、匹配（目标）点的索引（如果没有找到对应则为-1）、对应点之间的距离或代表对应关系估计的权重（匿名联合体）、无参构造函数（初始化成员变量，查询点索引为0，匹配点索引-1，距离初始化为float最大）、有参构造函数（根据传入的参数初始化成员变量）、虚析构函数为空（表明结构体是多态的，可以被继承）。结构体中使用了 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 宏，用于在堆上分配内存时确保内存对齐，以配合 Eigen 库的要求。

对点云应用变换矩阵进行变换（使用给定的变换矩阵对点云数据进行旋转、缩放和平移变换，并利用并行计算加快处理速度）： 如果传入的变换矩阵是单位矩阵，不进行变换直接返回，否则并行化for循环对点云中的每个点进行变换的操作将会在多个处理器核心上并行执行，以提高性能。

使用SVD方法计算去重心坐标点集的变换矩阵（输入去中心化后的源点云数据和目标点云数据，源点云的质心和目标点云的质心，计算得到的变换矩阵使用引用传递）：估计两组点云之间的变换矩阵，主要利用了奇异值分解（SVD）方法。  初始化变化矩阵为单位矩阵，计算源点云和目标点云的相关性矩阵 H（3X3），计算H的奇异值分解，得到U,V矩阵，判断 U 和 V 的行列式乘积是否小于 0，如果条件为真，则遍历矩阵 V 的第三列（索引为 2 的列），将其每个元素乘以 -1 来反转该列的方向。这是为了“取消”因为行列式乘积小于 0 而产生的潜在反射效果，保证最终的旋转矩阵 R = V * U' 不包含任何反射成分。计算旋转矩阵R=V*U' 。 将旋转矩阵 R 放入变换矩阵的左上角，计算平移向量，平移向量为目标点云质心减去旋转矩阵 R 作用在源点云质心上的结果。

通过法线计算旋转矩阵的函数（目标点云集合，输入点云集合，点对应关系集合-其中每个对应关系包含查询点索引、匹配点索引和两点之间的距离， 用于存储计算结果的变换矩阵，点对之间的平均距离，均方根误差，内点数量）：  获取对应点数量N，初始化源点和目标点矩阵4xN零矩阵，初始化质心4x1矩阵，设置源点云和目标点云质心为零向量，遍历对应关系集合，基于有效的点对（匹配点索引不为 -1）构建源点云和目标点云的矩阵，并同时计算两个点云的质心，统计内点数量（如果为0直接返回），计算均方根误差RMSE，点对之间的平均距离。去中心化处理，声明存储去质心后的点云，通过 OpenMP并行计算去中心化的点云数据。最后调用 前面的函数使用去中心化的点云和质心计算变换矩阵。

通过法线计算旋转矩阵的函数（初始向量，目标向量）： 根据两个向量创建一个旋转矩阵，使第一个向量旋转到与第二个向量对齐。归一化两个向量。计算两个向量的夹角。计算旋转轴，将旋转轴归一化。初始化旋转矩阵为单位矩阵。 根据rodrigues公式计算旋转矩阵（通过罗德里格斯公式填充旋转部分的元素值），返回计算得到的旋转矩阵。

计算点云的平整度（计算点云与最佳拟合平面的距离统计信息，能够有效评估点云的平整性）：  获取点云的大小，定义平面方程系数，定义质心和法线。调用通用工具函数类的平面方法获得质心和法线。输出质心坐标。计算平面方程的系数。输出平面方程系数。定义向量保存点到平面的距离，遍历点云{获取一个点，如果任一坐标值包含NaN则跳过。 计算点到平面距离，保存到距离向量} 。 如果距离数不足则直接返回。将距离排序。根据输入参数局外点百分比计算外点的数量。计算局外点数量一半[从距离向量中剔除这些最外围的点（排序后的最大和最小距离值），以减少极端值的影响]。定义剔除外点后的距离向量并重置该向量大小，使用memcpy将 距离数组中剔除最大和最小百分比边缘点后的中间部分拷贝到 移除局外点后的数组中。获取移除局外点后的最大距离和最小距离。计算平均距离。输出平均距离。