PCA主成分分析以及C++求解二维点法向量实例_pca 法向量

一家之言，仅作分享，如有不合理或需要改进的地方，欢迎各位讨论。
在解决点到线的ICP问题时，提到会提前计算target点云中每一个点的法向量，其中利用的方法就是PCA确定该点附近多个点的方差最大的主方向，再求解过该点的法向量。
在这里将介绍c++利用PCA求解二维点法向量的问题。

PCA方法简介

PCA(Principal Component Analysis)，即主成分分析方法，是一种使用最广泛的数据降维算法。降维具有如下一些优点：

1. 使得数据集更易使用。
2. 降低算法的计算开销。
3. 去除噪声。
4. 使得结果容易理解。

PCA的主要思想是将n维特征映射到k维上，这k维是全新的正交特征也被称为主成分，是在原有n维特征的基础上重新构造出来的k维特征。

主成分分析要解决的问题就是：如何找到一个轴，使得样本空间的所有点映射到这个轴的方差最大。

PCA流程

设有$m$条$n$维数据。

1. 将原始数据按列组成$n$行$m$列矩阵$X$；
2. 将$X$的每一行进行零均值化，即减去这一行的均值；
3. 求出协方差矩阵$C=\frac{1}{m}X{X}^T$；
4. 求出协方差矩阵的特征值及对应的特征向量；
5. 将特征向量按对应特征值大小从上到下按行排列成矩阵，取前$k$行组成矩阵$P$；
6. $Y=PX$即为降维到$k$维后的数据。

C++ Eigen库解决PCA问题

下面代码示例是结合nanoflann的KDtree查找最近邻点，对多个近邻点求解PCA问题，判断是否为一条直线，并确定某点的法向量，用于后续ICP问题costfunction构建。
对应可参考：
KDtree相关库nanoflann的应用
多种形式ICP问题的ceres实例应用

void ScanAlign::PCAPoints(CloudPoints &rCloudPoints)
{
    if(rCloudPoints.size() == 0)
        return;
    // ************** get norm_xy of each point.
    // constuct kdtree
    nanoflann::PointCloud<double> kdtree_pts;
    size_t N = rCloudPoints.size();
    kdtree_pts.pts.resize(N);
    for(size_t i=0; i< N; i++){
        CloudPoint pnt = rCloudPoints.at(i);
        kdtree_pts.pts.at(i).x = pnt.x;
        kdtree_pts.pts.at(i).y = pnt.y;
    }

    kd_tree_t kdtree(2 /*dim*/, kdtree_pts, nanoflann::KDTreeSingleIndexAdaptorParams(1 /* max leaf */));
    kdtree.buildIndex();

    for(size_t i=0; i<N; i++)
    {
        // do a knn search
        const double query_pt[2] = {rCloudPoints.at(i).x, rCloudPoints.at(i).y}; // 选中的特征点
        size_t num_results = 6;
        std::vector<size_t>   ret_index(num_results); // 搜索到按距离排序的点序号
        std::vector<double>   out_dist_sqr(num_results);  // 搜索到的点平方距离

        num_results = kdtree.knnSearch(&query_pt[0], num_results, &ret_index[0], &out_dist_sqr[0]);

        /***************************************
         * PCA(Principal component analysis)主成分分析:
         * 1.计算协方差矩阵的特征向量和特征值
         * 2.对特征向量进行排序，降序排序
         * 3.按照特征值的顺序对特征向量进行排序，组成变换矩阵【这里我们可以选取需要保留的维度个数】
         * 4.对原数据进行变换
         寻找最近的5个点，对点云协方差矩阵进行主成份分析：
         协方差矩阵最大特征值对应的特征向量的方向，就是数据变化最大的方向。
         若这5个点分布在直线上，协方差矩阵的特征值包含一个元素显著大于其余两个，与该特征值相关的特征向量表示所处直线的方向.
         ***************************************/
        if(out_dist_sqr.back() < 2)  //5个点中最大距离不超过2m才处理
        {
            std::vector<Eigen::Vector2d> nearPoints;     // 保存5个点的坐标
            Eigen::Vector2d center(0, 0);
            for (int j = 1; j < 6; j++)
            {
                Eigen::Vector2d tmp(kdtree_pts.pts.at(ret_index[j]).x,
                                    kdtree_pts.pts.at(ret_index[j]).y);
                center = center + tmp;
                nearPoints.push_back(tmp);
            }
            // 五个点的坐标的算术平均    即期望  Ex
            center = center / 5.0;
            /*
             *  计算协方差矩阵   ，注意协方差矩阵计算的是样本不同维度间的相关性   而不是样本之间的相关性
             *  这里有5个样本点 ，每个样本3维   协方差矩阵 = 1/(n-1) * (X-EX)(X-EX)^T ,   X = (x1,x2,x3,x4,x5)
             *  这里没有除 1/n-1 但关系不大，因为特征向量的方向不会改变
             * */
            Eigen::Matrix2d covMat = Eigen::Matrix2d::Zero();
            for (int j = 1; j < 6; j++)
            {
                Eigen::Matrix<double, 2, 1> tmpZeroMean = nearPoints[j] - center;
                covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
            }

            Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> saes(covMat);
            // if is indeed line feature
            // note Eigen library sort eigenvalues in increasing order
            Eigen::Vector2d unit_direction = saes.eigenvectors().col(1);    // 最大的特征值对应的特征向量
            // 如果最大的特征值  明显比第二大的大    则认为是直线
            if (saes.eigenvalues()[1] > 3 * saes.eigenvalues()[0])
            {
                // 该点的法向量
                rCloudPoints.at(i).norm_x = -unit_direction[1];
                rCloudPoints.at(i).norm_y = unit_direction[0];
            }
        }
    }
    //*********
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78