pcl源码分析之法向量

---

前言

点云处理中，法向量是一个很重要的参数，这里分析下pcl计算法向量的源码，加深对原理的理解。大概原理和流程可以参考这篇博文。

---

一、应用实例

#include <pcl/features/normal_3d_omp.h>//使用OMP需要添加的头文件

	pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;//OMP加速
	//pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	n.setNumberOfThreads(10);//设置openMP的线程数
	n.setViewPoint(0, 0, 0);//设置视点，默认为（0，0，0）
	n.setInputCloud(cloud);
	n.setSearchMethod(tree);

	if(is_radius)
		n.setRadiusSearch(radius);//半径搜素
	else
		n.setKSearch(sum);//点云法向计算时，需要所搜的近邻点大小
	
	n.compute(*normals);//开始进行法向计

二、NormalEstimationOMP类

在NormalEstimationOMP类中，核心函数是computeFeature。

三、computeFeature函数

该函数有两个个需要注意的地方：

1. for循环的多线程运行——#pragma omp parallel for；
2. 置nan值——std::numeric_limits::quiet_NaN ()；

//模板函数
template <typename PointInT, typename PointOutT> void
pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Allocate enough space to hold the results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  output.is_dense = true;

  // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
  if (input_->is_dense)
  {
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
    // Iterating over the entire index vector
    for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
    {
      //如果找不到近邻点，则将参数置为nan值
      //这里还是用到了kdtree，kdtree在pcl源码里的使用率非常高
      if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
      {
        output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

        output.is_dense = false;
        continue;
      }

      Eigen::Vector4f n;
      //计算单个点云的法向量
      pcl::computePointNormal<PointInT> (*surface_, nn_indices,
                                         n,
                                         output.points[idx].curvature);
                          
      output.points[idx].normal_x = n[0];
      output.points[idx].normal_y = n[1];
      output.points[idx].normal_z = n[2];
  	  
  	  //设置点云的视点，也即点云的朝向
      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output.points[idx].normal[0], 
                                  output.points[idx].normal[1], 
                                  output.points[idx].normal[2]);
    }
  }
  else
  {
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
     // Iterating over the entire index vector
    for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
    {
      if (!isFinite ((*input_)[(*indices_)[idx]]) ||
          this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
      {
        output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

        output.is_dense = false;
        continue;
      }

      Eigen::Vector4f n;
      pcl::computePointNormal<PointInT> (*surface_, nn_indices,
                                         n,
                                         output.points[idx].curvature);
                          
      output.points[idx].normal_x = n[0];
      output.points[idx].normal_y = n[1];
      output.points[idx].normal_z = n[2];

      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
                                  output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
    }
 }
}

四、computePointNormal函数

该函数主要由两个函数构成：computeMeanAndCovarianceMatrix和solvePlaneParameters。

1.computeMeanAndCovarianceMatrix

计算均值和协方差矩阵，可以回顾下计算协方差公式，也可参考1，2。

template <typename PointT, typename Scalar> inline unsigned int
pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                                     Eigen::Matrix<Scalar, 3, 3> &covariance_matrix,
                                     Eigen::Matrix<Scalar, 4, 1> &centroid)
{
  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
  Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 9, Eigen::RowMajor>::Zero ();
  size_t point_count;
  if (cloud.is_dense)
  {
    point_count = cloud.size ();
    // For each point in the cloud
    //计算每一个点的期望E
    for (size_t i = 0; i < point_count; ++i)
    {
      accu [0] += cloud[i].x * cloud[i].x;//E(X2)*N
      accu [1] += cloud[i].x * cloud[i].y;//E(XY)*N
      accu [2] += cloud[i].x * cloud[i].z;//E(XZ)*N
      accu [3] += cloud[i].y * cloud[i].y; // 4 //E(Y2)*N
      accu [4] += cloud[i].y * cloud[i].z; // 5 //E(YZ)*N
      accu [5] += cloud[i].z * cloud[i].z; // 8 //E(Z2)*N
      accu [6] += cloud[i].x; //E(X)2*N
      accu [7] += cloud[i].y; //E(Y)2*N
      accu [8] += cloud[i].z; //E(Z)2*N
    }
  }
  else
  {
    point_count = 0;
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      if (!isFinite (cloud[i]))
        continue;

      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
      accu [6] += cloud[i].x;
      accu [7] += cloud[i].y;
      accu [8] += cloud[i].z;
      ++point_count;
    }
  }
  accu /= static_cast<Scalar> (point_count);//这里再除以N得到真正的期望E
  if (point_count != 0)
  {
    //centroid.head<3> () = accu.tail<3> ();    -- does not compile with Clang 3.0
    //均值
    centroid[0] = accu[6]; centroid[1] = accu[7]; centroid[2] = accu[8];
    centroid[3] = 1;
    //对于3维变量，其协方差矩阵是个3*3的矩阵
    covariance_matrix.coeffRef (0) = accu [0] - accu [6] * accu [6];//cov(x,x)
    covariance_matrix.coeffRef (1) = accu [1] - accu [6] * accu [7];//cov(x,y)
    covariance_matrix.coeffRef (2) = accu [2] - accu [6] * accu [8];//cov(x,z)
    covariance_matrix.coeffRef (4) = accu [3] - accu [7] * accu [7];//cov(y,y)
    covariance_matrix.coeffRef (5) = accu [4] - accu [7] * accu [8];//cov(y,z)
    covariance_matrix.coeffRef (8) = accu [5] - accu [8] * accu [8];//cov(z,z)
    covariance_matrix.coeffRef (3) = covariance_matrix.coeff (1);//cov(y,x) = cov(x,y)
    covariance_matrix.coeffRef (6) = covariance_matrix.coeff (2);//cov(z,x) = cov(x,z)
    covariance_matrix.coeffRef (7) = covariance_matrix.coeff (5);//cov(z,y) = cov(y,z)
  }
  return (static_cast<unsigned int> (point_count));
}

2.solvePlaneParameters 函数

inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
                           const Eigen::Vector4f &point,
                           Eigen::Vector4f &plane_parameters, float &curvature)
{
  //函数重载
  solvePlaneParameters (covariance_matrix, plane_parameters [0], plane_parameters [1], plane_parameters [2], curvature);

  plane_parameters[3] = 0;
  // Hessian form (D = nc . p_plane (centroid here) + p)
  //Ax + By + Cz + D = 0;其中，这里是求D
  //已知法向量和其中一点，求平面方程就这么求，当然这里只需要求出法向量即可，这一步实际上可以省略
  plane_parameters[3] = -1 * plane_parameters.dot (point);  
}

inline void
pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix,
                           float &nx, float &ny, float &nz, float &curvature)
{
  // Avoid getting hung on Eigen's optimizers
//  for (int i = 0; i < 9; ++i)
//    if (!pcl_isfinite (covariance_matrix.coeff (i)))
//    {
//      //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values!\n");
//      nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
//      return;
//    }
  // Extract the smallest eigenvalue and its eigenvector
  EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
  EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
  //pcl自带的求特征值和特征向量函数，据说比eigen里的要精确一些
  pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);

  nx = eigen_vector [0];
  ny = eigen_vector [1];
  nz = eigen_vector [2];

  // Compute the curvature surface change
  float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8);
  if (eig_sum != 0)
    curvature = fabsf (eigen_value / eig_sum);
  else
    curvature = 0;
}

总结

总结了一下pcl里求取点云法向量的计算方法和代码流程，后续有新的发现和领悟再补充。