《PCL点云库学习&VS2010(X64)》Part 43 协方差矩阵的特征向量

《PCL点云库学习&VS2010(X64)》Part 43 协方差矩阵的特征向量

 

 

对协方差矩阵的特征向量最直观的解释之一是它总是指向数据方差最大的方向。

更准确地说，第一特征向量是数据方差最大的方向，第二特征向量是与第一特征向量垂直的方向上数据方差最大的方向，第三特征向量是与第一和第二特征向量垂直的方向上数据方差最大的方向，以此类推。

下图是二维空间的一个例子：

 

 

 

 

每个数据样本都是可以用坐标x、y表示的二维点。这些数据样本的协方差矩阵的特征向量是u和v。较长的u是第一特征向量，较短的v是第二特征向量。特征值的大小用箭头的长度表示。我们可以看到，第一个特征向量（从数据的平均值）指向欧几里德空间中数据方差最大的方向，第二个特征向量跟第一特征向量是垂直的。

三维空间中的特征向量就比较复杂，如图所示：

我们假设所有的数据点都在椭圆体内。v1是第一特征向量，λ1是其相应的特征值，指向数据方差最大的方向。v2与v1垂直，是这个方向上数据方差最大的特征向量。v3与v1和v2都垂直，是这个方向上数据方差最大的特征向量，虽然只有这一个方向。

//

根据上述可知，其数学上的意义很明了，λ1为其方差最大的方向，而方差最小的方方向肯定是与该方向垂直的，在空间中表现出来的物理意义也就很明显了，必定为其法线方向。

那么λ2与λ3哪一个是最小值呢，即哪一个λ对应的才是法线方向呢？两者中的最小值所对应的特征向量为其法线方向。因此在用Eigen矩阵求解时，一般会对特征值做一个排序（升序排列），最小的那个特征值（第一个特征值）为法线方向所对应的特征值，其特征向量表示法线方向。

//

对于三维空间点云而言，利用Eigen求解协方差矩阵后，特征值(向量)以升序方式排列，

Eigen::MatrixXd covMat = computeCovarianceMatrix();

Eigen::Matrix3d A = covMat;

Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;

es.compute(A);

//特征值特征向量以升o序排列

const auto& = eVec = es.eigenvectors();

//获取法向量

m_IsPlaneVector[2] = Eigen::Vector3d(eVec.col(0).data());//最小特征（值）向量，特征向量第一列，为点云的法向量

m_IsPlaneVector[0] = Eigen::Vector3d(eVec.col(2).data());//最大特征(值)向量

 

注：

1、得到的向量向量m_IsPlaneVector[2]为法线方向的特征向量，m_IsPlaneVector定义为Eigen::Vector3dm_IsPlaneVector[3] ，是二维数组，因此x、y、z中的法线分别对应为：

m_IsPlaneVector[2][0]

m_IsPlaneVector[2][1]

m_IsPlaneVector[2][2]

2、一般需要对特征向量v1,v2,v3归一化处理，一般先分别归一化其中两个，另外一个通过叉乘得到。

分别利用SVD和PCA两种方式求解法线：

SVD:

estimatePlane(
    const PointICloud& cloud_ground) {
    model_t model;

    // Create covariance matrix.
    // 1. calculate (x,y,z) mean
    float mean_x = 0., mean_y = 0., mean_z = 0.;
    for (size_t pt = 0u; pt < cloud_ground.points.size(); ++pt) {
        mean_x += cloud_ground.points[pt].x;
        mean_y += cloud_ground.points[pt].y;
        mean_z += cloud_ground.points[pt].z;
    }
    if (cloud_ground.points.size()) {
        mean_x /= cloud_ground.points.size();
        mean_y /= cloud_ground.points.size();
        mean_z /= cloud_ground.points.size();
    }
    // 2. calculate covariance
    // cov(x,x), cov(y,y), cov(z,z)
    // cov(x,y), cov(x,z), cov(y,z)
    float cov_xx = 0., cov_yy = 0., cov_zz = 0.;
    float cov_xy = 0., cov_xz = 0., cov_yz = 0.;
    for (int i = 0; i < cloud_ground.points.size(); i++) {
        cov_xx += (cloud_ground.points[i].x - mean_x) *
                  (cloud_ground.points[i].x - mean_x);
        cov_xy += (cloud_ground.points[i].x - mean_x) *
                  (cloud_ground.points[i].y - mean_y);
        cov_xz += (cloud_ground.points[i].x - mean_x) *
                  (cloud_ground.points[i].z - mean_z);
        cov_yy += (cloud_ground.points[i].y - mean_y) *
                  (cloud_ground.points[i].y - mean_y);
        cov_yz += (cloud_ground.points[i].y - mean_y) *
                  (cloud_ground.points[i].z - mean_z);
        cov_zz += (cloud_ground.points[i].z - mean_z) *
                  (cloud_ground.points[i].z - mean_z);
    }
    // 3. setup covariance matrix Cov
    Eigen::MatrixXf Cov(3, 3);
    Cov << cov_xx, cov_xy, cov_xz, cov_xy, cov_yy, cov_yz, cov_xz, cov_yz,
        cov_zz;
    Cov /= cloud_ground.points.size();

    // Singular Value Decomposition: SVD
    Eigen::JacobiSVD<Eigen::MatrixXf> SVD(
        Cov, Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    model.normal = (SVD.matrixU().col(2));
    // d is directly computed substituting x with s^ which is a good
    // representative for the points belonging to the plane
    Eigen::MatrixXf mean_seeds(3, 1);
    mean_seeds << mean_x, mean_y, mean_z;
    // according to normal^T*[x,y,z]^T = -d
    model.d = -(model.normal.transpose() * mean_seeds)(0, 0);

    // ROS_WARN_STREAM("Model: " << model.normal << " " << model.d);

    return model;
}

PCA: 

1 #define _CRT_SECURE_NO_WARNINGS
 2 #define _SCL_SECURE_NO_WARNINGS
 3 #include <pcl/io/pcd_io.h>
 4 #include <pcl/point_types.h>
 5 #include <pcl/features/normal_3d.h>
 6 #include <Eigen/core>
 7 
 8 #include <vector>
 9 
10 using namespace std;
11 
12 int main() 
13 {
14     pcl::PCDReader reader;
15     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
16     reader.read("../../data/23b_12.pcd", *cloud);
17 
18     int cld_sz = cloud->size();
19     pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
20     normals->resize(cld_sz);
21 
22     //计算中心点坐标
23     double center_x = 0, center_y = 0, center_z = 0;
24     for (int i = 0; i < cld_sz; i++) {
25         center_x += cloud->points[i].x;
26         center_y += cloud->points[i].y;
27         center_z += cloud->points[i].z;
28     }
29     center_x /= cld_sz;
30     center_y /= cld_sz;
31     center_z /= cld_sz;
32     //计算协方差矩阵
33     double xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
34     for (int i = 0; i < cld_sz; i++) {
35         xx += (cloud->points[i].x - center_x) * (cloud->points[i].x - center_x);
36         xy += (cloud->points[i].x - center_x) * (cloud->points[i].y - center_y);
37         xz += (cloud->points[i].x - center_x) * (cloud->points[i].z - center_z);
38         yy += (cloud->points[i].y - center_y) * (cloud->points[i].y - center_y);
39         yz += (cloud->points[i].y - center_y) * (cloud->points[i].z - center_z);
40         zz += (cloud->points[i].z - center_z) * (cloud->points[i].z - center_z);
41     }
42     //大小为3*3的协方差矩阵
43     Eigen::Matrix3f covMat(3, 3);
44     covMat(0, 0) = xx / cld_sz;
45     covMat(0, 1) = covMat(1, 0) = xy / cld_sz;
46     covMat(0, 2) = covMat(2, 0) = xz / cld_sz;
47     covMat(1, 1) = yy / cld_sz;
48     covMat(1, 2) = covMat(2, 1) = yz / cld_sz;
49     covMat(2, 2) = zz / cld_sz;
50 
51     //求特征值与特征向量
52     Eigen::EigenSolver<Eigen::Matrix3f> es(covMat);
53     Eigen::Matrix3f val = es.pseudoEigenvalueMatrix();
54     Eigen::Matrix3f vec = es.pseudoEigenvectors();
55 
56     //找到最小特征值t1
57     double t1 = val(0, 0);
58     int ii = 0;
59     if (t1 > val(1, 1)) {
60         ii = 1;
61         t1 = val(1, 1);
62     }
63     if (t1 > val(2, 2)){
64         ii = 2;
65         t1 = val(2, 2);
66     }
67 
68     //最小特征值对应的特征向量v_n
69     Eigen::Vector3f v(vec(0, ii), vec(1, ii), vec(2, ii));
70     //特征向量单位化
71     v /= v.norm();
72     　for (int i = 0; i < cld_sz; i++) {
74         normals->points[i].normal_x = v(0);
75         normals->points[i].normal_y = v(1);
76         normals->points[i].normal_z = v(2);
77         normals->points[i].curvature = t1 / (val(0, 0) + val(1, 1) + val(2, 2));
78     }
79 
80     cin.get();
81     return 0;
82 }

3.根据拟合的特征值特征向量求解平面方程：

 d = -(v[0] * mean[0] + v[1] * mean[1] + v[2] * mean[2]);  //平面方程 Ax+By+Cz+d = 0;V为最小特征值对应特征向量

4.根据局部坐标系进行局部坐标系投影变换

Reference:点云直线特征、平面特征、圆柱特征计算方法

  // Compute covariance   //For real matrices, conjugate() is a no-operation, and so adjoint() is equivalent to transpose().
  N 为法向量集合
  Eigen::MatrixXd cov = (N * N.adjoint()) / double(N.cols() - 1);     //样本方差 https://en.wikipedia.org/wiki/Covariance_matrix
    // PCA using QR decomposition for symmetric matrices
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(cov);
  Eigen::Vector3d S = es.eigenvalues();
  double score = S(2) / S(0);
  
    // Checkpoint 1
  if (score < cylinder_score_min) {
    return;
  }
 
  Eigen::Vector3d vec = es.eigenvectors().col(0);
  axis[0] = vec(0);
  axis[1] = vec(1);
  axis[2] = vec(2);
 
  Eigen::MatrixXd N_cpy = N.block(0, 0, 3, m);
  N = N_cpy; /* This avoids memory issues */
  Eigen::MatrixXd P_proj(3, m);
 
  // Projection to plane: P' = P-theta*<P.theta>
  Eigen::MatrixXd P_dot_theta = vec.transpose() * P;
  P_proj.row(0) = P.row(0) - P_dot_theta * vec(0);
  P_proj.row(1) = P.row(1) - P_dot_theta * vec(1);
  P_proj.row(2) = P.row(2) - P_dot_theta * vec(2);
  Eigen::MatrixXd N_dot_theta = vec.transpose() * N;
  N.row(0) -= N_dot_theta * vec(0);
  N.row(1) -= N_dot_theta * vec(1);
  N.row(2) -= N_dot_theta * vec(2);
 
  // Normalize projected normals
  Eigen::MatrixXd Normals_norm = N.colwise().norm();
  N.row(0) = N.row(0).array() / Normals_norm.array();
  N.row(1) = N.row(1).array() / Normals_norm.array();
  N.row(2) = N.row(2).array() / Normals_norm.array();