PCL 计算每个点的协方差矩阵（C++详细过程版）

@TOC

一、算法原理

1、概述

  计算点云中每个点的协方差矩阵是众多点云算法中极其重要的基础计算之一。PCL中没有自带的调用函数（可以通过一些函数的组合来实现）。为充分了解算法实现的每一个细节和有待改进的地方，使用C++代码对算法实现过程进行复现。

2、计算公式

  对于点云数据中的任意一点$p$，根据其邻域内点的坐标计算其协方差矩阵。计算公式如下：
M = ∑ i