从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方法。
1 template <typename PointInT, typename PointOutT> void pcl::Feature<PointInT, PointOutT>::compute (PointCloudOut &output) 2 { 3 if (!initCompute ()) 4 { 5 output.width = output.height = 0; 6 output.points.clear (); 7 return; 8 } 9 // Copy the header 10 output.header = input_->header; 11 // Resize the output dataset 12 if (output.points.size () != indices_->size ()) 13 output.points.resize (indices_->size ()); 14 // Check if the output will be computed for all points or only a subset 15 // If the input width or height are not set, set output width as size 16 if (indices_->size () != input_->points.size () || input_->width * input_->height == 0) 17 { 18 output.width = static_cast<uint32_t> (indices_->size ()); 19 output.height = 1; 20 } 21 else 22 { 23 output.width = input_->width; 24 output.height = input_->height; 25 } 26 output.is_dense = input_->is_dense; 27 // Perform the actual feature computation 28 computeFeature (output); 29 deinitCompute (); 30 }
2.注意computeFeature (output);方法 ，可以知道这是一个私有的虚方法。
private:
/** rief 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方法：
1 template <typename PointInT, typename PointOutT> void pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output) 2 { 3 // Allocate enough space to hold the results 4 // ote This resize is irrelevant for a radiusSearch (). 5 std::vector< int> nn_indices (k_); 6 std::vector< float> nn_dists (k_); 7 output.is_dense = true; 8 // Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense 9 if (input_->is_dense) 10 { 11 // Iterating over the entire index vector 12 for (size_t idx = 0; idx < indices_->size (); ++idx) 13 { 14 if (this ->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || 15 !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) 16 { 17 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 (); 18 output.is_dense = false; 19 continue; 20 } 21 22 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); 23 } 24 } 25 else 26 { 27 // Iterating over the entire index vector 28 for (size_t idx = 0; idx < indices_->size (); ++idx) 29 { 30 if (!isFinite ((*input_)[(*indices_)[idx]]) || 31 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 || 32 !computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature)) 33 { 34 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 (); 35 36 output.is_dense = false; 37 continue; 38 } 39 flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]); 40 } 41 } 42 }
4.因此分析NormalEstimation的算法流程如下：
（1）进行点云的初始化initCompute
（2）初始化计算结果输出对象output
（3）计算点云法向量，具体由子类的computeFeature方法实现。先搜索近邻searchForNeighbors ，然后计算computePointNormal
采用的方法是PCA主成分分析法。
参考文献：《Semantic 3D Object Maps for Everyday Manipulation in Human Living Environments》 P45-49
点云的法向量主要是通过点所在区域的局部拟合的表面进行计算。平面通过一个点和法向量进行表示。对于每一个点Pi,对应的协方差矩阵C
关于主成份分析的基本原理和算法流程参考：http://blog.csdn.net/lming_08/article/details/21335313
（4）flipNormalTowardsViewpoint 法向量定向，采用方法是：使法向量的方向朝向viewpoint。
5.NormalEstimation模板类的重载方法computeFeature分析，computePointNormal分析。
1 inline bool computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices, 2 float &nx, float &ny, float &nz, float &curvature) 3 { 4 if (indices.size () < 3 || 5 computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0) 6 { 7 nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); 8 return false; 9 } 10 11 // Get the plane normal and surface curvature 12 solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature); 13 return true; 14 }
computeMeanAndCovarianceMatrix主要是PCA过程中计算平均值和协方差矩阵，在类centroid.hpp中。
而solvePlaneParameters方法则是为了求解特征值和特征向量。代码见feature.hpp。具体实现时采用了pcl::eigen33方法。
1 inline void pcl::solvePlaneParameters (const Eigen::Matrix3f &covariance_matrix, 2 float &nx, float &ny, float &nz, float &curvature) 3 { 4 // Avoid getting hung on Eigen's optimizers 5 // for (int i = 0; i < 9; ++i) 6 // if (!pcl_isfinite (covariance_matrix.coeff (i))) 7 // { 8 // //PCL_WARN ("[pcl::solvePlaneParameteres] Covariance matrix has NaN/Inf values! "); 9 // nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN (); 10 // return; 11 // } 12 // Extract the smallest eigenvalue and its eigenvector 13 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; 14 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; 15 pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); 16 17 nx = eigen_vector [0]; 18 ny = eigen_vector [1]; 19 nz = eigen_vector [2]; 20 21 // Compute the curvature surface change 22 float eig_sum = covariance_matrix.coeff (0) + covariance_matrix.coeff (4) + covariance_matrix.coeff (8); 23 if (eig_sum != 0) 24 curvature = fabsf (eigen_value / eig_sum); 25 else 26 curvature = 0; 27 }
6.法向量定向
见normal_3d.h文件中，有多个覆写方法。摘其一：
1 /** rief Flip (in place) the estimated normal of a point towards a given viewpoint 2 * param point a given point 3 * param vp_x the X coordinate of the viewpoint 4 * param vp_y the X coordinate of the viewpoint 5 * param vp_z the X coordinate of the viewpoint 6 * param nx the resultant X component of the plane normal 7 * param ny the resultant Y component of the plane normal 8 * param nz the resultant Z component of the plane normal 9 * ingroup features 10 */ 11 template <typename PointT> inline void 12 flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, 13 float &nx, float &ny, float &nz) 14 { 15 // See if we need to flip any plane normals 16 vp_x -= point.x; 17 vp_y -= point.y; 18 vp_z -= point.z; 19 20 // Dot product between the (viewpoint - point) and the plane normal 21 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz); 22 23 // Flip the plane normal 24 if (cos_theta < 0) 25 { 26 nx *= -1; 27 ny *= -1; 28 nz *= -1; 29 } 30 }
运行的实例结果：
