PCL點云庫入門（第21講）——PCL庫點云特征之RSD特征描述Radius-based Surface Descriptor（RSD）

一、算法原理

RSD: Radius-based Surface Descriptor由 Marton Zsolt et al. 于 2010 年提出，主要用于 點云中物體的幾何形狀識別（如球形、柱面、平面等），廣泛用于機器人抓取、點云分割和物體識別等任務中。
在這里插入圖片描述

1.1、RSD 特征的核心思想

RSD 特征的基本思想是：通過分析每個點在其鄰域中構成的三角形幾何關系，估算最小與最大曲率半徑（min/max radius），從而推測該點鄰域的局部表面類型。

基本定義：

對每個點 $p$ ，RSD 給出兩個描述子：

最小曲率半徑 $r_{\min}$ ：表示該點局部區域中最小的“曲率”尺度（可能是棱角、邊緣）；
最大曲率半徑 $r_{\max}$ ：表示該點周圍整體結構的尺度（如圓柱或球面的大致半徑）；

這兩個值可用于判斷：

幾何類型	$r_{\min}$ 與 $r_{\max}$ 特征關系
平面	兩者都為無窮大（或極大）
圓柱面	一個大，一個小（如 $r_{\min} \ll r_{\max}$ )
球面	兩者相等且有限
邊緣/角	$r_{\min}$ 很小（尖銳特征）

1.2、算法流程

Step1:獲取鄰域

對于查詢點 $p$ ，在給定半徑 $r$ 下搜索其鄰域點集 $\mathcal{N}(p)$ 。

Step2:計算點對間幾何關系

對于鄰域內的每一對點 $p_i, p_j)$ ，計算與查詢點 $p$ 構成的角度和距離：

設三點構成三角形，計算兩邊夾角（通過向量法），進而估計曲率半徑：

$\frac{\| p_i - p_j \|}{2 \sin(\theta)}$
- $\theta$ ：為兩個邊與法線之間的夾角
- 該公式源于圓弧的幾何關系，假設曲面為圓形/球面片段

Step3:聚合統計值

遍歷所有點對組合后，統計：
- 所有半徑中的最小值 → $r_{\min}$
- 所有半徑中的最大值 → $r_{\max}$

Step4:判別表面類型（可選）

根據經驗或閾值將點分類為：
- 平面
- 圓柱面
- 球面
- 棱邊/尖銳角

1.3、優點

計算簡單、快速：只用到幾何關系，無需復雜擬合；
無需先驗模型：適用于未知物體表面識別；
適用于粗略分類：如判斷點是否位于平面、邊緣或曲面；
可集成到抓取點檢測、分割前處理等任務中。

二、主要成員變量和成員函數

2.1、成員變量（數據成員）

int nr_subdiv_

含義：距離角度直方圖中的距離區間劃分數目。
用途：RSD特征中構造距離-角度直方圖時使用。

** double plane_radius_**

含義：平面半徑閾值。大于這個半徑的點對被視為“共面”。
建議設置：大概是搜索半徑的10–20倍，默認 0.2。
用途：用于區分曲面和平面。

** bool save_histograms_**

含義：是否保存完整的距離-角度直方圖。
用途：對調試或分析特征構造很有用。

shared_ptr<std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf>>> histograms_

含義：保存所有點的完整距離-角度直方圖的容器。
類型：每個點對應一個 Eigen::MatrixXf（浮點型矩陣），用來存直方圖。
條件：僅當 save_histograms_ == true 時填充。

2.2、成員函數

構造函數

RSDEstimation() : nr_subdiv_(5), plane_radius_(0.2), save_histograms_(false)

默認設置：
- nr_subdiv_ = 5
- plane_radius_ = 0.2
- save_histograms_ = false
- 并設置 feature_name_ = "RadiusSurfaceDescriptor"

設置參數的函數

`setNrSubdivisions(int nr_subdiv)`

設置直方圖中距離區間的子分段數量。

`getNrSubdivisions()`

獲取子分段數量。

`setPlaneRadius(double plane_radius)`

設置將區域視為“平面”的最大半徑。

`getPlaneRadius()`

獲取“平面”判定閾值。

`setSaveHistograms(bool save)`

設置是否保存每個點的距離-角度直方圖。

`getSaveHistograms()`

判斷是否保存直方圖。

`getHistograms()`

返回保存的直方圖列表（若啟用保存功能）。

特別重定義函數

`setKSearch(int)`

setKSearch (int) {PCL_ERROR ("RSD does not work with k nearest neighbor search. Use setRadiusSearch() instead!\n");
}

明確禁止用戶用 KNN 搜索。
RSD 依賴固定半徑搜索而非最近鄰數，因此此函數直接報錯。

重載函數

`computeFeature(PointCloudOut &output)`

該函數重寫自 Feature，執行具體的 RSD 特征計算。
參數 output 是輸出的特征點云（每個點含有 r_min, r_max 半徑估計）。
實現體未給出（通常在 .cpp 中定義）。

繼承體系說明（部分引用）

class RSDEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>

說明這個類繼承自帶法線特征估計的基類：

FeatureFromNormals<PointInT, PointNT, PointOutT> ?? 繼承自 Feature<PointInT, PointOutT>

你看到的這幾行 using 是為了讓父類的 protected 或 public 成員可見：

using Feature<PointInT, PointOutT>::feature_name_;
using Feature<PointInT, PointOutT>::indices_;
...

類型定義（typedef）

using PointCloudOut = typename Feature<PointInT, PointOutT>::PointCloudOut;
using PointCloudIn = typename Feature<PointInT, PointOutT>::PointCloudIn;

為簡化代碼，引入輸入輸出點云類型別名。

using Ptr = shared_ptr<RSDEstimation<...>>;
using ConstPtr = shared_ptr<const RSDEstimation<...>>;

引入智能指針類型別名，方便聲明與管理內存。

computeRSD 核心函數（多個重載）

原型一（主版本）：

template <typename PointInT, typename PointNT, typename PointOutT>
Eigen::MatrixXf computeRSD(const pcl::PointCloud<PointInT> &surface,const pcl::PointCloud<PointNT> &normals,const std::vector<int> &indices,double max_dist,int nr_subdiv,double plane_radius,PointOutT &radii,bool compute_histogram = false);

功能：

對給定點（使用 indices 中的第一個點作為參考點），從其法線和鄰域點坐標中估計 RSD。
輸出 r_min、r_max 并可選生成二維距離-角度直方圖。

原型二（簡化版本，無坐標點，僅使用法線 + sqr_dists）：

template <typename PointNT, typename PointOutT>
Eigen::MatrixXf computeRSD(const pcl::PointCloud<PointNT> &normals,const std::vector<int> &indices,const std::vector<float> &sqr_dists,double max_dist,int nr_subdiv,double plane_radius,PointOutT &radii,bool compute_histogram = false);

說明：

該版本在某些預處理已完成（如 sqr_dists 已知）時可直接使用，加速處理。

原型三和四（過時的 shared_ptr 版本）：

template <...>
[[deprecated]]
Eigen::MatrixXf computeRSD(typename pcl::PointCloud<PointInT>::ConstPtr &surface,...);

說明：

已標記為 [[deprecated]]，建議改為傳引用的函數調用方式，避免使用 shared_ptr。

三、關鍵部分代碼注釋

** 1、計算單個點的RSD**

template <typename PointNT, typename PointOutT>
Eigen::MatrixXf pcl::computeRSD(const pcl::PointCloud<PointNT> &normals,const std::vector<int> &indices,const std::vector<float> &sqr_dists,double max_dist,int nr_subdiv,double plane_radius,PointOutT &radii,bool compute_histogram)

函數簽名：

功能：給定一個點（第一個索引處）和它的鄰居點索引 indices，根據其法線角度與距離的分布，估計 r_min 和 r_max 兩個曲率半徑值。
參數解釋：
- normals：包含所有點的法線向量。
- indices：目標點的鄰居點的索引（第一個索引作為參考點）。
- sqr_dists：對應點對之間的平方距離。
- max_dist：最大允許距離，超過此距離的鄰居點將被忽略。
- nr_subdiv：角度和距離劃分的子區間個數。
- plane_radius：最大半徑閾值，如果計算結果超過該值就認為是平面。
- radii：結果存放結構體，包含 r_min 與 r_max。
- compute_histogram：是否保存完整的角度-距離直方圖。

template <typename PointNT, typename PointOutT> Eigen::MatrixXf
pcl::computeRSD (const pcl::PointCloud<PointNT> &normals,const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
{// 1. 定義一個二維矩陣 histogram 用于保存角度-距離直方圖，如果需要計算直方圖則初始化為零矩陣Eigen::MatrixXf histogram;if (compute_histogram)histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);// 2. 判斷鄰居點數量是否足夠，至少需要兩個點才能計算特征，否則將輸出半徑設置為0，返回空直方圖if (indices.size () < 2){radii.r_max = 0;radii.r_min = 0;return histogram;}// 3. 為每個距離bin準備一個保存最小角度和最大角度的容器，大小為nr_subdivstd::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);// 4. 第一個距離bin初始化為0角度范圍（兩值相等）min_max_angle_by_dist[0].resize (2);min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;// 5. 其他距離bin初始化為極端值（方便后續更新最小最大角度）for (int di=1; di<nr_subdiv; di++){min_max_angle_by_dist[di].resize (2);min_max_angle_by_dist[di][0] = +DBL_MAX;  // 最小角度初始化為最大浮點數min_max_angle_by_dist[di][1] = -DBL_MAX;  // 最大角度初始化為最小浮點數}// 6. 遍歷鄰居點，從第二個點開始（第一個點作為參考點）std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());for (i = begin+1; i != end; ++i){// 7. 計算當前點與第一個點的法向量夾角（不考慮法向量方向，只考慮夾角大小）double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] +normals.points[*i].normal[1] * normals.points[*begin].normal[1] +normals.points[*i].normal[2] * normals.points[*begin].normal[2];// 防止計算誤差導致的cosine超出[-1, 1]范圍if (cosine > 1) cosine = 1;if (cosine < -1) cosine = -1;double angle  = std::acos (cosine);if (angle > M_PI/2) angle = M_PI - angle; // 角度限定在[0, π/2]，忽略方向// 8. 計算點與參考點的距離（平方根）double dist = sqrt (sqr_dists[i-begin]);// 9. 忽略超出最大距離閾值的點if (dist > max_dist)continue;// 10. 計算距離對應的bin索引int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));// 11. 如果需要計算直方圖，則計算角度bin，并在對應的二維bin上計數加一if (compute_histogram){int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));histogram(bin_a, bin_d)++;}// 12. 更新當前距離bin中對應的最小角度和最大角度if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;}// 13. 用最小角度線和最大角度線來估計曲率半徑，定義一些累積變量double Amint_Amin = 0, Amint_d = 0;double Amaxt_Amax = 0, Amaxt_d = 0;// 14. 對每個距離bin進行累加，計算最小和最大角度對應的矩陣乘積項（為后續線性擬合準備）for (int di=0; di<nr_subdiv; di++){if (min_max_angle_by_dist[di][1] >= 0){double p_min = min_max_angle_by_dist[di][0];double p_max = min_max_angle_by_dist[di][1];double f = (di+0.5)*max_dist/nr_subdiv;  // bin中心對應的距離值Amint_Amin += p_min * p_min;Amint_d += p_min * f;Amaxt_Amax += p_max * p_max;Amaxt_d += p_max * f;}}// 15. 通過最小二乘估計計算半徑，如果無有效數據則用plane_radius替代，并且限制最大半徑為plane_radiusfloat min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));// 16. 對估計結果進行修正，減小系統誤差（此系數基于實驗經驗nr_subdiv=5時得出）min_radius *= 1.1f;max_radius *= 0.9f;// 17. 賦值給輸出半徑結構，保證r_min < r_maxif (min_radius < max_radius){radii.r_min = min_radius;radii.r_max = max_radius;}else{radii.r_max = min_radius;radii.r_min = max_radius;}// 18. 返回計算得到的角度-距離直方圖矩陣return histogram;
}

額外說明：

該函數基于點云鄰居點的法線夾角與距離分布，構建了一個 2D 直方圖（角度 vs 距離）。
通過對直方圖最小最大角度沿距離bin做線性擬合，估計點云局部的曲率半徑（r_min 和 r_max）。
r_min 和 r_max 能夠描述點云局部曲面可能的曲率范圍，便于后續點云特征提取與分類。
compute_histogram 標志決定是否返回完整的直方圖，用于調試或更細致分析。

** 2、計算點云的RSD**

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::RSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{// 1. 檢查是否設置了搜索半徑（search_radius_），若未設置（<0），則報錯并清空輸出點云if (search_radius_ < 0){PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());output.width = output.height = 0;output.points.clear ();return;}// 2. 用于存放鄰域搜索返回的點索引和對應的距離平方值std::vector<int> nn_indices;std::vector<float> nn_sqr_dists;// 3. 判斷是否需要保存完整的直方圖數據if (save_histograms_){// 4. 初始化直方圖的存儲容器，使用 Eigen 的對齊分配器保證內存對齊histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);histograms_->reserve (output.points.size ());// 5. 遍歷所有輸入點索引，針對每個點計算特征for (std::size_t idx = 0; idx < indices_->size (); ++idx){// 6. 使用搜索參數（search_parameter_）對當前點執行鄰域搜索，得到鄰居點索引及距離平方this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);// 7. 調用 computeRSD 函數計算 RSD（Radius-based Surface Descriptor）特征//    并保存完整直方圖數據（最后一個參數設為 true）histograms_->push_back (computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));}}else{// 8. 若不保存直方圖，遍歷每個輸入點索引計算特征for (std::size_t idx = 0; idx < indices_->size (); ++idx){// 9. 依然執行鄰域搜索this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);// 10. 調用 computeRSD 計算特征，但不保存直方圖（最后一個參數設為 false）computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);}}
}

函數說明：

該函數是 RSDEstimation 類中用于計算每個點的 Radius-based Surface Descriptor (RSD) 特征的核心接口。
它會針對 indices_ 中的每個點，執行鄰域搜索，找到鄰居點，并根據鄰居點的法線信息計算特征。
computeRSD 函數返回的結果會被存儲在輸出點云 output 中的對應點（output.points[idx]）。
若 save_histograms_ 為真，額外還會保存每個點的角度-距離直方圖矩陣，便于后續分析或調試。

四、使用代碼示例

/*****************************************************************//*** \file   PCLLearnRSDFeathermain.cpp* \brief  * * \author ZhengShi.Yang* \date   June 2025*********************************************************************/#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/rsd.h>
#include <pcl/search/kdtree.h>
#include <iostream>int TestRSD(int argc, char** argv)
{// 1. 讀取點云數據pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);std::string file = "E:/db_data/PCLLearnData/cloud_bin_1.pcd";if (pcl::io::loadPCDFile<pcl::PointXYZ>(file, *cloud) == -1){PCL_ERROR("Couldn't read input file input.pcd\n");return -1;}std::cout << "Loaded point cloud with " << cloud->size() << " points.\n";// 2. 計算法線pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;ne.setInputCloud(cloud);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);ne.setSearchMethod(tree);ne.setRadiusSearch(0.03);  // 根據點云密度調整搜索半徑ne.compute(*normals);std::cout << "Computed normals.\n";// 3. 計算RSD特征pcl::PointCloud<pcl::PrincipalRadiiRSD>::Ptr rsd_features(new pcl::PointCloud<pcl::PrincipalRadiiRSD>);pcl::RSDEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalRadiiRSD> rsd;rsd.setInputCloud(cloud);rsd.setInputNormals(normals);rsd.setSearchMethod(tree);rsd.setRadiusSearch(0.05);  // RSD搜索半徑，影響特征尺度rsd.compute(*rsd_features);std::cout << "Computed RSD features for " << rsd_features->size() << " points.\n";// 4. 輸出部分結果查看for (size_t i = 0; i < std::min<size_t>(5, rsd_features->size()); ++i){std::cout << "Point " << i << ": r_min = " << rsd_features->points[i].r_min<< ", r_max = " << rsd_features->points[i].r_max << std::endl;}}int main(int argc, char** argv)
{TestRSD(argc, argv);std::cout << "Hello PCL World!" << std::endl;std::system("pause");return 0;
}