[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方法。
 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:
      /** \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方法：
 1 template <typename PointInT, typename PointOutT> void pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
 2 {
 3   // Allocate enough space to hold the results
 4   // \note 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!\n");
 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  /** \brief 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   }

运行的实例结果：

 

posted @ 2016-01-22 13:55  太一吾鱼水  阅读(17788)  评论(0)  编辑  收藏  举报