[PCL]2 點雲法向量計算NormalEstimation

從GitHub的代碼版本庫下載源代碼https://github.com/PointCloudLibrary/pcl，用CMake生成VS項目，查看PCL的源碼位於pcl_features項目下

1.Feature類：

template <typename PointInT, typename PointOutT>   class Feature : public PCLBase<PointInT>

注意 Feature是一個泛型類，有一個compute方法。

template <typename PointInT, typename PointOutT> void pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output)
{
  if (!initCompute ())
  {
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }
  // Copy the header
  output.header = input_->header;
  // Resize the output dataset
  if (output.points.size () != indices_->size ())
    output.points.resize (indices_->size ());
  // Check if the output will be computed for all points or only a subset
  // If the input width or height are not set, set output width as size
  if (indices_->size () != input_->points.size () || input_->width * input_->height == 0)
  {
    output.width = static_cast<uint32_t> (indices_->size ());
    output.height = 1;
  }
  else
  {
    output.width = input_->width;
    output.height = input_->height;
  }
  output.is_dense = input_->is_dense;
  // Perform the actual feature computation
  computeFeature (output);
  deinitCompute ();
}

2.注意computeFeature (output);方法 ，可以知道這是一個私有的虛方法。

 private:

      /** \brief Abstract feature estimation method.

        * \param[out] output the resultant features    */

      virtual void    computeFeature (PointCloudOut &output) = 0;

3.查看Feature的繼承關系可以知道

template <typename PointInT, typename PointOutT>   class NormalEstimation: public Feature<PointInT, PointOutT>

NormalEstimation類是Feature模板類的子類，因此執行的是NormalEstimation類的computeFeature方法。查看computeFeature方法：

template <typename PointInT, typename PointOutT> void pcl::NormalEstimation<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)
  {
    // Iterating over the entire index vector
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (this ->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
          !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
      {
        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;
      }
 
      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
    }
  }
  else
  {
    // Iterating over the entire index vector
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      if (!isFinite ((*input_)[(*indices_)[idx]]) ||
          this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
          !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature))
      {
        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;
      } 
      flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,  output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);
    }
  }
}

4.因此分析NormalEstimation的算法流程如下：

 　  （1）進行點雲的初始化initCompute

　　（2）初始化計算結果輸出對象output

　　（3）計算點雲法向量，具體由子類的computeFeature方法實現。先搜索近鄰searchForNeighbors ，然后計算computePointNormal

　　　　采用的方法是PCA主成分分析法。

　　　　參考文獻：《Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments》 P45-49

　　　　　　　　　　http://www.pclcn.org/study/shownews.php?lang=cn&id=105

　　　　點雲的法向量主要是通過點所在區域的局部擬合的表面進行計算。平面通過一個點和法向量進行表示。對於每一個點Pi,對應的協方差矩陣C

　　　　

　　　　關於主成份分析的基本原理和算法流程參考：http://blog.csdn.net/lming_08/article/details/21335313

　　（4）flipNormalTowardsViewpoint 法向量定向，采用方法是：使法向量的方向朝向viewpoint。

5.NormalEstimation模板類的重載方法computeFeature分析，computePointNormal分析。

 inline bool computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
                          float &nx, float &ny, float &nz, float &curvature)
      {
        if (indices.size () < 3 ||
            computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
        {
          nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
          return false;
        }

        // Get the plane normal and surface curvature
        solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
        return true;
      }

computeMeanAndCovarianceMatrix主要是PCA過程中計算平均值和協方差矩陣，在類centroid.hpp中。
而solvePlaneParameters方法則是為了求解特征值和特征向量。代碼見feature.hpp。具體實現時采用了pcl::eigen33方法。

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::eigen33 (covariance_matrix, eigen_value, eigen_vector); 16 
  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;
}

 6.法向量定向

見normal_3d.h文件中，有多個覆寫方法。摘其一：

 /** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
    * \param point a given point
    * \param vp_x the X coordinate of the viewpoint
    * \param vp_y the X coordinate of the viewpoint
    * \param vp_z the X coordinate of the viewpoint
    * \param nx the resultant X component of the plane normal
    * \param ny the resultant Y component of the plane normal
    * \param nz the resultant Z component of the plane normal
    * \ingroup features
    */
  template <typename PointT> inline void
  flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z,
                              float &nx, float &ny, float &nz)
  {
    // See if we need to flip any plane normals
    vp_x -= point.x;
    vp_y -= point.y;
    vp_z -= point.z;

    // Dot product between the (viewpoint - point) and the plane normal
    float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);

    // Flip the plane normal
    if (cos_theta < 0)
    {
      nx *= -1;
      ny *= -1;
      nz *= -1;
    }
  }

運行的實例結果：