一、算法原理

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;
}