PCL：從法線計算到曲率計算並可視化

----------------法線求解原理--------------

表面法線是幾何體表面的重要屬性，在很多領域都有大量應用，例如：在進行光照渲染時產生符合可視習慣的效果時需要表面法線信息才能正常進行，對於一個已知的幾何體表面，根據垂直於點表面的矢量，因此推斷表面某一點的法線方向通常比較簡單。然而，由於我們獲取的點雲數據集在真實物體的表面表現爲一組定點樣本，這樣就會有兩種解決方法：

（1）使用曲面重建技術，從獲取的點雲數據集中得到採樣點對應的曲面，然後從曲面模型中計算表面法線；曲面方程f(x,y,z)=0的一個法向量可以表示爲n={e(df/dx), e(df/dy), e(df/dz)}.）

（2）直接從點雲數據集中近似推斷表面法線。

下面以已知一個點雲數據集，在其中的每個點處直接近似計算表面法線。爲例進行展開：

確定表面一點法線的問題近似於估計表面的一個相切面法線的問題，因此轉換過來以後就變成一個最小二乘法平面擬合估計問題。

最小二乘法（又稱最小平方法）是一種數學優化技術。它通過最小化誤差的平方和尋找數據的最佳函數匹配。利用最小二乘法可以簡便地求得未知的數據，並使得這些求得的數據與實際數據之間誤差的平方和爲最小。最小二乘法還可用於曲線擬合。其他一些優化問題也可通過最小化能量或最大化熵用最小二乘法來表達。

對空間中的一系列散點，尋求一個近似平面：（按最小原則選擇的擬合平面爲最小二乘擬合平面）

上述使用最小二乘我們已經可以根據某點的臨近點閾，可以擬合出一個平面。接下來就是求平面的法線。表面法線是幾何體面的重要屬性。而點雲數據集在真實物體的表面表現爲一組定點樣本。對點雲數據集的每個點的法線估計，可以看作是對錶面法線的近似推斷。在PCL庫中有專門針對法向量計算的庫，但是必須瞭解計算原理才能記得更加深刻。

確定表面一點法線的問題近似於估計表面的一個相切面法線的問題，因此轉換過來以後就變成一個最小二乘法平面擬合估計問題。

下一張圖說明怎麼是由求法線轉換到求最小特徵值對應的特徵向量就是代表法線。

類似於上圖中的平面方程求解，只是換了一種平面表示方法。（由一般式轉換爲法線式方程）

由上述推導過程可知，求法線就是求最小特徵值對應的特徵向量。

----

還可以觀察上述右邊圖片中的A矩陣其實是個協方差矩陣；

在PCL內估計一點集對應的協方差矩陣，可以使用以下函數調用實現：

//定義每個表面小塊的3x3協方差矩陣的存儲對象
Eigen::Matrix3fcovariance_matrix;
//定義一個表面小塊的質心座標16-字節對齊存儲對象
Eigen::Vector4fxyz_centroid;
//估計質心座標
compute3DCentroid(cloud,xyz_centroid);
//計算3x3協方差矩陣
computeCovarianceMatrix(cloud,xyz_centroid,covariance_matrix);

通常，沒有數學方法能解決法線的正負向問題，如上所示，通過主成分分析法（PCA）來計算它的方向也具有二義性，無法對整個點雲數據集的法線方向進行一致性定向。圖1顯示出對一個更大數據集的兩部分產生的影響，此數據集來自於廚房環境的一部分，很明顯估計的法線方向並非完全一致，圖2展現了其對應擴展的高斯圖像（EGI），也稱爲法線球體（normal sphere），它描述了點雲中所有法線的方向。由於數據集是2.5維，其只從一個單一的視角獲得，因此法線應該僅呈現出一半球體的擴展高斯圖像（EGI）。然而，由於定向的不一致性，它們遍佈整個球體，如圖2所示。

如果實際知道視點 $V_{p}$ ，那麼這個問題的解決是非常簡單的。對所有 $\vec{n}$ 法線定向只需要使它們一致朝向視點方向，滿足下面的方程式：

$\vec{n}\cdot (V_{p}-V_{i})>0$

下面的圖3展現了上面圖1中的數據集的所有法線被一致定向到視點後的結果演示。

在PCL中對一個已知點的法線進行手動重新定向，你可以使用：

flipNormalTowardsViewpoint (const PointT &point, float vp_x, float vp_y, float vp_z, Eigen::Vector4f &normal);

注意：如果數據集是從多個捕獲視點中配準後集成的，那麼上述法線的一致性定向方法就不適用了。需要使用更復雜的算法。更詳細的信息見：http://pointclouds.org/documentation/tutorials/how_features_work.php#id1

選擇合適的尺度

如之前介紹的，在估計一個點的表面法線時，我們需要從周圍支持這個點的鄰近點着手（也稱作k鄰域）。最近鄰估計問題的具體內容又提出了另一個問題“合適的尺度”：已知一個取樣點雲數據集，k的正確取值是多少（k通過pcl::Feature::setKSearch給出）或者確定一個點r爲半徑的圓內的最近鄰元素集時使用的半徑r應該取什麼值（r通過pcl::Feature::setRadiusSearch給出）。這個問題非常重要，並且在一個點特徵算子的自動估計時（例如：用戶沒有給定閾值）是一個限制因素。爲了更好地說明這個問題，以下圖示表現了選擇更小尺度（如：r值或k取相對小）與選擇更大尺度（如：r值或k值比較大）時的兩種不同效果。圖4和圖5分別爲近視圖和遠視圖，兩圖中左邊部分展示選擇了一個合理的比例因子，估計的表面法線近似垂直於兩個平面，即使在互相垂直的邊沿部分，可明顯看到邊沿。如果這個尺度取的太大（右邊部分），這樣鄰近點集將更大範圍地覆蓋鄰近表面的點，估計的點特徵表現就會扭曲失真，在兩個平面邊緣處出現旋轉表面法線，以及模糊不清的邊界，這樣就隱藏了一些細節信息。

無法深入探究更多討論，現在可粗略假設，以應用程序所需的細節需求爲參考，選擇確定點的鄰域所用的尺度。簡言之，如果杯子手柄和圓柱體部分之間邊緣的曲率是重要的，那麼需要足夠小的尺度來捕獲這些細節信息，而在其他不需要細節信息的應用中可選擇大的尺度。

估計法線實例詳解：http://pointclouds.org/documentation/tutorials/index.php#features-tutorial

法線估計類NormalEstimation的實際計算調用程序內部執行以下操作：

對點雲P中的每個點p
  1.得到p點的最近鄰元素
  2.計算p點的表面法線n
  3.檢查n的方向是否一致指向視點，如果不是則翻轉

視點座標默認爲（0,0,0），可以使用以下代碼進行更換：

setViewPoint (float vpx, float vpy, float vpz);

計算單個點的法線，使用：

computePointNormal (const pcl::PointCloud<PointInT>&cloud, const std::vector<int>&indices, Eigen::Vector4f &plane_parameters, float&curvature);

此處，cloud是包含點的輸入點雲，indices是點的k-最近鄰元素集索引，plane_parameters和curvature是法線估計的輸出，plane_parameters前三個座標中，以(nx, ny, nz)來表示法線，第四個座標D = nc . p_plane (centroid here) + p。輸出表面曲率curvature通過協方差矩陣的特徵值之間的運算估計得到，如：

$\sigma =\frac{\lambda _{0}}{\lambda _{0}+\lambda _{1}+\lambda _{2}}$

使用OpenMP加速法線估計

對於對運算速度有要求的用戶，PCL點雲庫提供了一個表面法線的附加實現程序，它使用多核/多線程開發規範，利用OpenMP來提高計算速度。它的類命名爲pcl::NormaleEstimationOMP，並且它的應用程序接口（API）100%兼容單線程pcl::NormalEstimation，這使它適合作爲一個可選提速方法。在8核系統中，可以輕鬆提速6-8倍。

-------------------曲率求解-----------------

曲率大小反映模型表面的凹凸程度。

利用上面法向量估計的PCA方法，在法向量估算的基礎上進行數據點的曲率估算，曲率的估算方法如上式：

$\lambda _{0}$ 入描述了曲面沿法向量的變化，而 $\lambda _{1}$ 和 $\lambda _{2}$ 表示數據點在切平面上的分佈情況。定義下式爲數據點 $g _{i}$ ，在k鄰域內的曲面變分。

$\sigma =\frac{\lambda _{0}}{\lambda _{0}+\lambda _{1}+\lambda _{2}}$

點雲模型在數據點的曲率 $H_{i}$ 可近似爲在該點的曲面變分 $\sigma$ ,即 $H_{i}$ $\approx$ $\sigma$ 。

該式的基本思想實際上是使用曲面變分來近似曲率信息[1]。

曲率基礎知識：
平均曲率、主曲率和高斯曲率是曲率的三個基本要素。
法曲率：曲面在一點沿着不同方向的彎曲程度不同。或者說曲面離開切平面的速度不同。這個彎曲屬性可以用這一點的沿着這個方法的法曲率刻畫
主曲率：過曲面上某個點上具有無窮個正交曲率，其中存在一條曲線使得該曲線的曲率爲極大，這個曲率爲極大值Kmax，垂直於極大曲率面的曲率爲極小值Kmin。這兩個曲率屬性爲主曲率。他們代表着法曲率的極值。
平均曲率：是空間上曲面上某一點任意兩個相互垂直的正交曲率的平均值。如果一組相互垂直的正交曲率可表示爲K1,K2,那麼平均曲率則爲：K = (K1 +K2 ) / 2。
高斯曲率：兩個主曲率的乘積即爲高斯曲率，又稱總曲率，反映某點上總的完全程度。

#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/principal_curvatures.h>
using namespace std;

int main (int argc, char** argv){

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile<pcl::PointXYZ>("A3  - Cloud.pcd", *cloud); //讀取點雲
  cout << "Loaded " << cloud->points.size() << " points." << std::endl;//顯示讀取點雲的點數
  // 計算點雲的法線
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
  normalEstimation.setInputCloud (cloud);
  //設置鄰域點搜索方式
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  normalEstimation.setSearchMethod (tree);
  //定義一個新的點雲儲存含有法線的值
  pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>);
  //設置KD樹搜索半徑
 // normalEstimation.setRadiusSearch (0.03);
  normalEstimation.setKSearch(10);
  //計算出來法線的值
  normalEstimation.compute (*cloudWithNormals);
  // 建立主曲率計算
  pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> principalCurvaturesEstimation;
  // 提供原始點雲(沒有法線)
  principalCurvaturesEstimation.setInputCloud (cloud);
  // 爲點雲提供法線
  principalCurvaturesEstimation.setInputNormals(cloudWithNormals);
  // 使用與法線估算相同的KdTree
  principalCurvaturesEstimation.setSearchMethod (tree);
  //principalCurvaturesEstimation.setRadiusSearch(1.0);
  principalCurvaturesEstimation.setKSearch(10);
  // 計算主曲率
  pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principalCurvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
  principalCurvaturesEstimation.compute (*principalCurvatures);
  std::cout << "output points.size: " << principalCurvatures->points.size () << std::endl;
  // 顯示和檢索第0點的主曲率。
  cout << "最大麴率;"<< principalCurvatures->points[0].pc1 << endl;//輸出最大麴率
  cout << "最小曲率:"<< principalCurvatures->points[0].pc2 << endl;//輸出最小曲率
  //輸出主曲率方向（最大特徵值對應的特徵向量）
  cout << "主曲率方向;" << endl;
  cout << principalCurvatures->points[0].principal_curvature_x << endl;
  cout << principalCurvatures->points[0].principal_curvature_y << endl;
  cout << principalCurvatures->points[0].principal_curvature_z << endl;

  return 0;
}

[1]

PCL：從法線計算到曲率計算並可視化

選擇合適的尺度

使用OpenMP加速法線估計

PCA（1）：基礎知識介紹

【物料鋼卷識別與定位】：1.讀取數據（獲取原始數據）

矩陣的SVD分解（理論到計算結果）

PCL調錯（2）：VTK報錯

SVD與PCA的區別

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結