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

参考论文：Fast Cylinder and Plane Extraction from Depth Cameras for Visual Odometry

1.去除点云不连续区域（距离跳跃较大）；

2.平面特性：

1. 平面法向量由点云协方差最小的特征值对应的特征向量给出（平面特征中，总有一个特征值明显小于另两个特征值）；
2. 平面均方差(MSE)由点云协方差最小特征值给出；
3. 平面性评估由点云协方差第二大特征值与最小特征的比值给出，如果足够大则认为是平面；
4. 点云协方差计算示例：

  

  x_acc = X_matrix.sum();
  y_acc = Y_matrix.sum();
  z_acc = Z_matrix.sum();
  xx_acc = (X_matrix.array() * X_matrix.array()).sum();
  yy_acc = (Y_matrix.array() * Y_matrix.array()).sum();
  zz_acc = (Z_matrix.array() * Z_matrix.array()).sum();
  xy_acc = (X_matrix.array() * Y_matrix.array()).sum();
  xz_acc = (X_matrix.array() * Z_matrix.array()).sum();
  yz_acc = (Y_matrix.array() * Z_matrix.array()).sum();

  mean[0] = x_acc / nr_pts;
  mean[1] = y_acc / nr_pts;
  mean[2] = z_acc / nr_pts;
  // Expressing covariance as E[PP^t] + E[P]*E[P^T] //??  应该是E[PP^t] - E[P]*E[P^T]
  double cov[3][3] = {{xx_acc - x_acc * x_acc / nr_pts, xy_acc - x_acc * y_acc / nr_pts, xz_acc - x_acc * z_acc / nr_pts},
                      {0, yy_acc - y_acc * y_acc / nr_pts, yz_acc - y_acc * z_acc / nr_pts},
                      {0, 0, zz_acc - z_acc * z_acc / nr_pts}};
  cov[1][0] = cov[0][1];
  cov[2][0] = cov[0][2];
  cov[2][1] = cov[1][2];
 
  // This uses QR decomposition for symmetric matrices
  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es(Eigen::Map<Eigen::Matrix3d>(cov[0], 3, 3));
  Eigen::VectorXd v = es.eigenvectors().col(0);   // 最小特征值对应的特征向量
 
  d = -(v[0] * mean[0] + v[1] * mean[1] + v[2] * mean[2]);  //平面方程 Ax+By+Cz+d = 0;
  // Enforce normal orientation
  if (d > 0) {
    normal[0] = v[0];
    normal[1] = v[1];
    normal[2] = v[2];
  } else {
    normal[0] = -v[0];
    normal[1] = -v[1];
    normal[2] = -v[2];
    d = -d;
  }
  MSE = es.eigenvalues()[0] / nr_pts;
  score = es.eigenvalues()[1] / es.eigenvalues()[0];  //平面性评估

3.圆柱特性：

1. 在无噪声的圆柱表面中，法线协方差的最小特征值总是0；
2. 区域宽度影响法线协方差的第二大特征值，使用最大特征值与最小特征值的比值作为圆柱判断条件，如：

 Eigen::Vector3d S = es.eigenvalues();
  double score = S(2) / S(0);
  if (score < cylinder_score_min) { //cylinder_score_min=100
    return;
  }

1. 圆柱轴心 由法线协方差的最小特征值对应的特征向量给出；
2. 可使用投影方法将特征投影到垂直轴心的平面，做圆的拟合处理：

                                P'=Pi - v(v.pi)  其中Pi为待拟合平面片段的质心，P为投影平面的点，v为圆柱轴心向量,括号内为向量点乘
                                N'=Ni - v(v.Ni)

1. 点云协方差计算示例：

// 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();

4.直线特征：

1. 直线特征的点云协方差的特征值中总有一个特征值明显大于另两个特征值；
2. 直线方向由点云协方差最大的特征值对应的特征向量给出；