[PCL]2 點雲法向量計算NormalEstimation
阿新 • • 發佈：2018-11-01
參考：http://www.cnblogs.com/yhlx125/p/5137850.html
從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
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分析。
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 }
執行的例項結果：
