PCL点云库入门——PCL库点云特征之SHOT特征描述Signature of Histograms of OrienTations (SHOT)
1、算法原理
SHOT，即Signature of Histograms of OrienTations，是一种基于局部表面特征统计信息的特征描述原理。该方法通过计算点云中每个点的邻域内法向量的分布情况，生成一个多维的特征向量。这种方法的核心在于，
首先对每个点定义一个局部参考框架，以确保特征描述的旋转不变性。接着，在这个局部参考框架内，将邻域划分为多个子区域，并在每个子区域内统计法向量的分布情况进行编码。通过将这些统计信息组合成一个特征向量，SHOT能够有效地捕捉点云表面的局部几何特征。这种描述方法不仅对旋转具有鲁棒性，还能在一定程度上抵抗噪声和遮挡的影响，因此被广泛应用于点云配准、三维物体识别等领域。主要的内容步骤如下：
1）、建立局部坐标系（LRF）
局部坐标系的建立基于Hoppe等人提出的，利用K领域点云数据进行局部拟合最小二乘问题以计算点云法向量的方法，具体内容可以参考第八节的内容。在该方法中，法线方向的TLS估计由M对应最小特征值的特征向量给出，M如图1内容。最后，利用引导点对法线进行全局的方向符号奇异性统一，即通过从一个启发式选择的种子点传播方向符号。尽管这种方法已被证明是重建单个物体表面的健壮且有效技术，但它对于局部表面描述并不那么有效，因为在法向量方向确定中，方向符号必须在任何可能的物体姿态以及包含多个物体的场景中保持可重复性，因此需要一种局部而非全局的符号消歧方法。此外，Hoppe符号消歧仅关注法线，因此它使得其他两个轴的符号变得模糊不清。基于上述原因对M的计算进行改进，形式如图2。
图1
图2
M计算改进的内容如下：
（1）、修改了M以分配较远点更小的权重，以便在有杂乱存在的情况下增加可重复性。
（2）、为了提高对噪声的鲁棒性，用于计算描述符的所有位于球形支持区域（半径为R）内的点都被用来计算M。
（3）、为了提高效率，忽略了质心的计算，用查询点Pq来替代。因此，将M计算为加权线性组合。
在存在噪声和杂乱的情况下，M的特征向量定义了可重复的、正交的方向。值得注意的是，与Hoppe相比，对于改进的计算，第三个特征向量不再代表TLS对法线方向的估计，有时它与之明显不同。然而，这并不影响性能，因为在局部表面描述中，定义一组高度可重复且稳健的正交方向比其几何或拓扑意义更为重要。在下文中，按照特征值递减的顺序，将这三个单位特征向量分别称为x+、y+和z+轴。而x−、y−和z−则分别表示相反的单位向量。
M特征向量为局部坐标系提供了可重复的方向，但需要消除歧义以得到一个唯一的局部坐标系。EVD和SVD的符号消除问题得到了解决。基本上重新定位了每个奇异向量或特征向量，使其符号与它所代表的大多数向量一致。根据这个原则确定了局部x轴和z轴的符号。具体操作为：统计和
点的个数，将点数多的轴作为
轴，若两者点数相同，再去比较
和
的大小，具体内容如下：
M(k)为支撑域内每个点与特征点的距离最接近平均距离d的K个点的子集，即：
为了当|Sx+| = |Sx−|的地方消除EVD的歧义，只考虑M(k)奇数个K，得到子集S˜x+和S˜x−，并重新定向特征向量，使其符号与这些向量的大多数一致。同样用于消除z轴的歧义。最后，y轴是通过Z x X获得的。自此，完成LRF坐标系的建立。
2）、SHOT特征描述
（1）、 特征描述选择
在每个局部坐标系构建的直方图，根据点的法线Nq与特征点Zk处的在局部z轴之间的角度θq的cosθq余弦值进行投票。使用余弦值的原因有两个：首先，它可以快速计算，因为cos θq = Zk · Nq；其次，对cosθq进行等间隔的划分等同于对θq进行空间变化的划分，在参考法线方向附近的划分较粗，而正交方向的划分较细。通过这种方式，与法线正交方向上的小差异（可能是最有意义信息量的方向）会导致点被累积到不同的区间中，从而产生不同的直方图。此外，在准平面区域（不太具有描述性的区域）存在时，这种选择通过将计数集中在较少数量的区间中，限制了由于噪声引起的直方图差异。
（2）、 空间划分
对于标签特征结构，使用一个各向同性的球形支撑区域来描述，它沿着径向、方位角和仰角轴划分，如图3所示，由8个方位角分割、2个高度分割和2个径向分割得到的32个空间期间（为了方便查看，在图2中只显示了4个方位角分割）。
图3
每个空间直方图的被划分为11个bins，则得到的总特征描述符维度为11 x 32=352维，在PCL库中的定义的内容如下：
struct SHOT352
{
float descriptor[352];
float rf[9];
static int descriptorSize () { return 352; }
friend std::ostream& operator << (std::ostream& os, const SHOT352& p);
};（3）、特征描述生成
标签特征描述符是基于局部直方图的，因此避免边界效应非常重要。此外，由于标签结构，由于局部坐标系的扰动，边界效应也可能发生。因此，对于每个累积到特定局部直方图中的点，采用与其邻域进行四线性插值来弱化边缘效应，即局部直方图中的相邻区间以及具有相同索引的局部直方图中的区间，这些局部直方图对应于空间网格的相邻细分。每个区间bin的增量为每个维度的投票值1-d。对于局部直方图，d是当前区间与下一个区间中心值的距离。对于高度和方位角，d是块与体积中心值的角距离。对于径向维度，d是块与体积中心值的欧几里得距离。在每个维度上，d以直方图或空间网格间距的单位长度，即它通过两个相邻块（区间）或体积之间的距离进行归一化。四维性插值，对每一个点直方图投票时，四个维度根据距离权重进行插值。在同一个区域中，划分为11个区间，从cosθ维度进行插值，在相邻的区域根据d（角距离或欧式距离）进行插值投票，即就是每个点进行radial, azimuth, elevation三个维度的8个小区域的插值投票，每个小区域根据cosθ维度对两区间进行插值投票。四个维度插值如下：
<1>、法向量余弦值插值
假如当前点支撑区域内一点的特征值为cosθ ，在 （cosθi，cosθi+1)区间之间，第一步分别计算出 cosθ到 cosθi和cosθi+1 的归一化距离，标记为di和di+1,第二步cosθi区间的投票为+1-di，cosθi+1
区间的投票值为+1-di+1。线性插值的作用就是把当前值按照线性比例分配到相邻的离散区间上。示例如图4所示。
图4
<2>、经度插值
权重（比例）d 为角距离（angular distance）插值方法同法向量余弦值插值。示例如图5所示。
图5
<3>、维度插值
<4>、径向插值
权重 d 为欧式距离。
2、主要成员函数和变量
1、主要的成员变量
1、每个现状直方图的划分数，默认10
int nr_shape_bins_;2、单个点的SHOT特征信息缓存值
Eigen::VectorXf shot_;3、估计局部坐标系的半径值
float lrf_radius_;4、K邻域搜索半径值的平方
double sqradius_;5、K邻域搜索半径值的3/4
double radius3_4_;6、K邻域搜索半径值的1/4
double radius1_4_;7、K邻域搜索半径值的1/2
double radius1_2_;8、特征描述空间划分数，默认32
const int nr_grid_sector_;9、扇形划分数，默认32
const int maxAngularSectors_;10、SHOT特征描述的维度
int descLength_; 2、主要的成员函数
1、设置用于局部坐标系估计的半径
virtual void
setLRFRadius (float radius) { lrf_radius_ = radius; }2、四维线性插值
void
interpolateSingleChannel (const std::vector<int> &indices,
const std::vector<float> &sqr_dists,
const int index,
std::vector<double> &binDistance,
const int nr_bins,
Eigen::VectorXf &shot);3、特征直方图归一化
void
normalizeHistogram (Eigen::VectorXf &shot, int desc_length);4、创建一个区间距离直方图
void
createBinDistanceShape (int index, const std::vector<int> &indices,
std::vector<double> &bin_distance_shape);5、估计一个给定点及其空间邻域的3D点与法向量的SHOT特征描述
virtual void
computePointSHOT (const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_dists,
Eigen::VectorXf &shot) = 0;6、算法执行需要的相关参数初始化，如计算单点的局部坐标系构建
bool
initCompute () override; 3、主要实现代码注解
1、创建单点局部坐标系
//1、创建局部坐标系等算法条件初始化
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute()
{
if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute())
{
PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
return (false);
}
// K邻域方法检测
if (this->getKSearch() != 0)
{
PCL_ERROR(
"[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str());
return (false);
}
//创建每一点的局部参考系，具体参考下面内容
typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
lrf_estimator->setRadiusSearch((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
lrf_estimator->setInputCloud(input_);
lrf_estimator->setIndices(indices_);
if (!fake_surface_)
lrf_estimator->setSearchSurface(surface_);
if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames(indices_->size(), lrf_estimator))
{
PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
return (false);
}
return (true);
}
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF(const int& current_point_idx, Eigen::Matrix3f& rf)
{
const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap();
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
//最近邻域搜索，将结果保存到n_indices和n_sqr_distances中
this->searchForNeighbors(current_point_idx, search_parameter_, n_indices, n_sqr_distances);
//初始化化矩阵大小
Eigen::Matrix<double, Eigen::Dynamic, 4> vij(n_indices.size(), 4);
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
//构建协方差矩阵M
for (std::size_t i_idx = 0; i_idx < n_indices.size(); ++i_idx)
{
Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap();
if (pt.head<3>() == central_point.head<3>())
continue;
// 去中心操作，用查询点的值替代中心点
vij.row(valid_nn_points).matrix() = (pt - central_point).cast<double>();
vij(valid_nn_points, 3) = 0;
//计算权重 distance
distance = search_parameter_ - sqrt(n_sqr_distances[i_idx]);
// 协方差矩阵构建C=vij * vij'，加入距离权重
cov_m += distance * (vij.row(valid_nn_points).head<3>().transpose() * vij.row(valid_nn_points).head<3>());
sum += distance;
valid_nn_points++;
}
//构建局部坐标系的有效点至少为5个
if (valid_nn_points < 5)
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant(std::numeric_limits<float>::quiet_NaN());
return (std::numeric_limits<float>::max());
}
cov_m /= sum;
//协方差矩阵分解
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(cov_m);
const double& e1c = solver.eigenvalues()[0];
const double& e2c = solver.eigenvalues()[1];
const double& e3c = solver.eigenvalues()[2];
//特征值有效性判断
if (!std::isfinite(e1c) || !std::isfinite(e2c) || !std::isfinite(e3c))
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant(std::numeric_limits<float>::quiet_NaN());
return (std::numeric_limits<float>::max());
}
// 构建局部坐标系
Eigen::Vector4d v1 = Eigen::Vector4d::Zero();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero();
v1.head<3>().matrix() = solver.eigenvectors().col(2);
v3.head<3>().matrix() = solver.eigenvectors().col(0);
//局部坐标系x,z方向的确定
// 根据X，Z坐标系向量与协方差的余弦值值大于0的个数来确定
int plusNormal = 0, plusTangentDirection1 = 0;
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij.row(ne).dot(v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij.row(ne).dot(v3);
if (dp >= 0)
plusNormal++;
}
//切向方向确定
plusTangentDirection1 = 2 * plusTangentDirection1 - valid_nn_points;
if (plusTangentDirection1 == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points / 2;
for (int i = -points / 2; i <= points / 2; i++)
if (vij.row(medianIndex - i).dot(v1) > 0)
plusTangentDirection1++;
if (plusTangentDirection1 < points / 2 + 1)
v1 *= -1;
}
else if (plusTangentDirection1 < 0)
v1 *= -1;
//法向方向确定
plusNormal = 2 * plusNormal - valid_nn_points;
if (plusNormal == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points / 2;
for (int i = -points / 2; i <= points / 2; i++)
if (vij.row(medianIndex - i).dot(v3) > 0)
plusNormal++;
if (plusNormal < points / 2 + 1)
v3 *= -1;
}
else if (plusNormal < 0)
v3 *= -1;
rf.row(0).matrix() = v1.head<3>().cast<float>();
rf.row(2).matrix() = v3.head<3>().cast<float>();
rf.row(1).matrix() = rf.row(2).cross(rf.row(0));
return (0.0f);
}
//
template <typename PointInT, typename PointOutT> void
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::computeFeature(PointCloudOut& output)
{
//K邻域搜索方法检测
if (this->getKSearch() != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str());
return;
}
//搜索的结果根据距离来排序
tree_->setSortedResults(true);
for (std::size_t i = 0; i < indices_->size(); ++i)
{
// point result
Eigen::Matrix3f rf;
PointOutT& output_rf = output[i];
//计算每一个点的局部RF，结果保存到矩阵RF中
if (getLocalRF((*indices_)[i], rf) == std::numeric_limits<float>::max())
{
output.is_dense = false;
}
//保存结果到输出点云数据结果中
for (int d = 0; d < 3; ++d)
{
output_rf.x_axis[d] = rf.row(0)[d];
output_rf.y_axis[d] = rf.row(1)[d];
output_rf.z_axis[d] = rf.row(2)[d];
}
}
}2、创建局部坐标系等算法条件初始化
//1、创建局部坐标系等算法条件初始化
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute()
{
if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute())
{
PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
return (false);
}
// K邻域方法检测
if (this->getKSearch() != 0)
{
PCL_ERROR(
"[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str());
return (false);
}
//创建每一点的局部参考系，具体参考下面内容
typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
lrf_estimator->setRadiusSearch((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
lrf_estimator->setInputCloud(input_);
lrf_estimator->setIndices(indices_);
if (!fake_surface_)
lrf_estimator->setSearchSurface(surface_);
if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames(indices_->size(), lrf_estimator))
{
PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
return (false);
}
return (true);
}
3、创建距离符号直方图
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape(
int index,
const std::vector<int>& indices,
std::vector<double>& bin_distance_shape)
{
//初始化距离模型容器
bin_distance_shape.resize(indices.size());
//获取当前点的局部坐标系信息
const PointRFT& current_frame = frames_->points[index];
//if (!std::isfinite (current_frame.rf[0]) || !std::isfinite (current_frame.rf[4]) || !std::isfinite (current_frame.rf[11]))
//return;
//Z方向坐标
Eigen::Vector4f current_frame_z(current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
unsigned nan_counter = 0;
for (std::size_t i_idx = 0; i_idx < indices.size(); ++i_idx)
{
// 获取当前的点的法向量和NAN检测
const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap();
if (!std::isfinite(normal_vec[0]) ||
!std::isfinite(normal_vec[1]) ||
!std::isfinite(normal_vec[2]))
{
//法向量无效，距离模型值为NAN
bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN();
++nan_counter;
}
else
{
//double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
//计算cos θq = Zk · Nq距离余弦值
double cosineDesc = normal_vec.dot(current_frame_z);
//归一化值在[-1,1]之间
if (cosineDesc > 1.0)
cosineDesc = 1.0;
if (cosineDesc < -1.0)
cosineDesc = -1.0;
//计算最终的距离模型容器（分块）值，nr_shape_bins_=10
bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
}
}
if (nan_counter > 0)
PCL_WARN("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
getClassName().c_str(), index, nan_counter, (static_cast<float>(nan_counter) * 100.f / static_cast<float>(indices.size())));
}4、特征直方图归一化
///3、特征直方图归一化
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::normalizeHistogram(
Eigen::VectorXf& shot, int desc_length)
{
// Normalization is performed by considering the L2 norm
// and not the sum of bins, as reported in the ECCV paper.
// This is due to additional experiments performed by the authors after its pubblication,
// where L2 normalization turned out better at handling point density variations.
double acc_norm = 0;
for (int j = 0; j < desc_length; j++)
acc_norm += shot[j] * shot[j];
acc_norm = sqrt(acc_norm);
for (int j = 0; j < desc_length; j++)
shot[j] /= static_cast<float> (acc_norm);
}5、四维插值
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSingleChannel(
const std::vector<int>& indices,
const std::vector<float>& sqr_dists,
const int index,
std::vector<double>& binDistance,
const int nr_bins/*10*/,
Eigen::VectorXf& shot)
{
//当前点的坐标值和局部参考坐标系
//用当前点替代中心点
const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap();
const PointRFT& current_frame = (*frames_)[index];
//局部参考系坐标轴方向
Eigen::Vector4f current_frame_x(current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
Eigen::Vector4f current_frame_y(current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
Eigen::Vector4f current_frame_z(current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
for (std::size_t i_idx = 0; i_idx < indices.size(); ++i_idx)
{
if (!std::isfinite(binDistance[i_idx]))
continue;
//当前点相对中心点的平移量
Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap() - central_point;
delta[3] = 0;
// Compute the Euclidean norm
double distance = sqrt(sqr_dists[i_idx]);
//跳个自身点
if (areEquals(distance, 0.0))
continue;
//(Pq-pi)在局部参考系下三个方向的投影量
double xInFeatRef = delta.dot(current_frame_x);
double yInFeatRef = delta.dot(current_frame_y);
double zInFeatRef = delta.dot(current_frame_z);
// 过滤掉垂直情况
if (std::abs(yInFeatRef) < 1E-30)
yInFeatRef = 0;
if (std::abs(xInFeatRef) < 1E-30)
xInFeatRef = 0;
if (std::abs(zInFeatRef) < 1E-30)
zInFeatRef = 0;
unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
assert(bit3 == 0 || bit3 == 1);
int desc_index = (bit4 << 3) + (bit3 << 2);
desc_index = desc_index << 1;
if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
desc_index += (std::abs(xInFeatRef) >= std::abs(yInFeatRef)) ? 0 : 4;
else
desc_index += (std::abs(xInFeatRef) > std::abs(yInFeatRef)) ? 4 : 0;
desc_index += zInFeatRef > 0 ? 1 : 0;
// 2 RADII
desc_index += (distance > radius1_2_) ? 2 : 0;
int step_index = static_cast<int>(std::floor(binDistance[i_idx] + 0.5));
int volume_index = desc_index * (nr_bins + 1);
//插值结果公式，参见上面的图
//法向量余弦值插值
binDistance[i_idx] -= step_index;
double intWeight = (1 - std::abs(binDistance[i_idx]));
if (binDistance[i_idx] > 0)
shot[volume_index + ((step_index + 1) % nr_bins)] += static_cast<float> (binDistance[i_idx]);
else
shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += -static_cast<float> (binDistance[i_idx]);
//径向插值
if (distance > radius1_2_) //external sphere
{
double radiusDistance = (distance - radius3_4_) / radius1_2_;
if (distance > radius3_4_) //most external sector, votes only for itself
intWeight += 1 - radiusDistance; //peso=1-d
else //3/4 of radius, votes also for the internal sphere
{
intWeight += 1 + radiusDistance;
shot[(desc_index - 2) * (nr_bins + 1) + step_index] -= static_cast<float> (radiusDistance);
}
}
else //internal sphere
{
double radiusDistance = (distance - radius1_4_) / radius1_2_;
if (distance < radius1_4_) //most internal sector, votes only for itself
intWeight += 1 + radiusDistance; //weight=1-d
else //3/4 of radius, votes also for the external sphere
{
intWeight += 1 - radiusDistance;
shot[(desc_index + 2) * (nr_bins + 1) + step_index] += static_cast<float> (radiusDistance);
}
}
//维度插值
double inclinationCos = zInFeatRef / distance;
if (inclinationCos < -1.0)
inclinationCos = -1.0;
if (inclinationCos > 1.0)
inclinationCos = 1.0;
double inclination = std::acos(inclinationCos);
assert(inclination >= 0.0 && inclination <= PST_RAD_180);
if (inclination > PST_RAD_90 || (std::abs(inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
{
double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
if (inclination > PST_RAD_135)
intWeight += 1 - inclinationDistance;
else
{
intWeight += 1 + inclinationDistance;
assert((desc_index + 1) * (nr_bins + 1) + step_index >= 0 && (desc_index + 1) * (nr_bins + 1) + step_index < descLength_);
shot[(desc_index + 1) * (nr_bins + 1) + step_index] -= static_cast<float> (inclinationDistance);
}
}
else
{
double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
if (inclination < PST_RAD_45)
intWeight += 1 + inclinationDistance;
else
{
intWeight += 1 - inclinationDistance;
assert((desc_index - 1) * (nr_bins + 1) + step_index >= 0 && (desc_index - 1) * (nr_bins + 1) + step_index < descLength_);
shot[(desc_index - 1) * (nr_bins + 1) + step_index] += static_cast<float> (inclinationDistance);
}
}
if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
{
//经度插值
double azimuth = std::atan2(yInFeatRef, xInFeatRef);
int sel = desc_index >> 2;
double angularSectorSpan = PST_RAD_45;
double angularSectorStart = -PST_RAD_PI_7_8;
double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan * sel)) / angularSectorSpan;
assert((azimuthDistance < 0.5 || areEquals(azimuthDistance, 0.5)) && (azimuthDistance > -0.5 || areEquals(azimuthDistance, -0.5)));
azimuthDistance = (std::max)(-0.5, std::min(azimuthDistance, 0.5));
if (azimuthDistance > 0)
{
intWeight += 1 - azimuthDistance;
int interp_index = (desc_index + 4) % maxAngularSectors_;
assert(interp_index * (nr_bins + 1) + step_index >= 0 && interp_index * (nr_bins + 1) + step_index < descLength_);
shot[interp_index * (nr_bins + 1) + step_index] += static_cast<float> (azimuthDistance);
}
else
{
int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
assert(interp_index * (nr_bins + 1) + step_index >= 0 && interp_index * (nr_bins + 1) + step_index < descLength_);
intWeight += 1 + azimuthDistance;
shot[interp_index * (nr_bins + 1) + step_index] -= static_cast<float> (azimuthDistance);
}
}
assert(volume_index + step_index >= 0 && volume_index + step_index < descLength_);
shot[volume_index + step_index] += static_cast<float> (intWeight);
}
}
6、计算单点的SHOT特征信息
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT(
const int index, const std::vector<int>& indices, const std::vector<float>& sqr_dists, Eigen::VectorXf& shot)
{
//最近邻域少于5个的直接返回
if (indices.size() < 5)
{
PCL_WARN("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
getClassName().c_str(), (*indices_)[index]);
shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN());
return;
}
std::vector<double> binDistanceShape;
//创建当前点的距离模型容器，结果保存到binDistanceShape中
this->createBinDistanceShape(index, indices, binDistanceShape);
// 线性插值计算SHOT特征信息
shot.setZero();
// nr_shape_bins_=10;
interpolateSingleChannel(indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
// 归一化直方图的信息
this->normalizeHistogram(shot, descLength_);
}
7、计算SHOT特征描述的总接口流程
//5、计算SHOT特征描述的总接口流程
//
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature(pcl::PointCloud<PointOutT>& output)
{
//特征描述的维度352=32*（10+1）
descLength_ = nr_grid_sector_ * (nr_shape_bins_ + 1);
//半径邻域搜索的平方值，3/4,1/4,1/2的半径值
sqradius_ = search_radius_ * search_radius_;
radius3_4_ = (search_radius_ * 3) / 4;
radius1_4_ = search_radius_ / 4;
radius1_2_ = search_radius_ / 2;
assert(descLength_ == 352);
//特征描述的初始化为352个0
shot_.setZero(descLength_);
//邻域搜索初始化
std::vector<int> nn_indices(k_);
std::vector<float> nn_dists(k_);
output.is_dense = true;
// 对每一个点执行计算
for (std::size_t idx = 0; idx < indices_->size(); ++idx)
{
//当前点对应的局部坐标系有效判断
bool lrf_is_nan = false;
const PointRFT& current_frame = (*frames_)[idx];
if (!std::isfinite(current_frame.x_axis[0]) ||
!std::isfinite(current_frame.y_axis[0]) ||
!std::isfinite(current_frame.z_axis[0]))
{
PCL_WARN("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
getClassName().c_str(), (*indices_)[idx]);
lrf_is_nan = true;
}
//当前点有效性和最近邻域搜索检测
if (!isFinite((*input_)[(*indices_)[idx]]) ||
lrf_is_nan ||
this->searchForNeighbors((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
//当前点无效或者最近邻域搜索不满足条件时，输出的特征信息为NAN值
for (int d = 0; d < descLength_; ++d)
output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN();
for (int d = 0; d < 9; ++d)
output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN();
output.is_dense = false;
continue;
}
// 计算单个点的SHOT特征信息
computePointSHOT(static_cast<int> (idx), nn_indices, nn_dists, shot_);
// 将计算的结果保存到输出的点云数据中，
//包含SHOT特征描述和当前点的局部坐标系信息
for (int d = 0; d < descLength_; ++d)
output.points[idx].descriptor[d] = shot_[d];
for (int d = 0; d < 3; ++d)
{
output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
}
}
}4、使用代码示例
/*****************************************************************//**
* \file PCSHOTFeaturemain.cpp
* \brief
*
* \author YZS
* \date January 2025
*********************************************************************/
#include<iostream>
#include <vector>
#include <ctime>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/auto_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/shot_lrf.h>
#include <pcl/features/shot.h>//包含SHOT计算的模块
using namespace std;
void PCLSHOTFeature()
{
//加载点云数据
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
std::string fileName = "E:/fragment.pcd";
pcl::io::load(fileName, *cloud);
std::cout << "Cloud Size:" << cloud->points.size() << std::endl;
//计算点云数据的法向量
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch(0.04);
ne.compute(*cloud_normals);
//计算点云数据的SHOT特征信息
//pcl::SHOTEstimation SHOT特征估计对象
pcl::SHOTEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::SHOT352> shot_estimation;
// 设置需要计算SHOT的点云数据
shot_estimation.setInputCloud(cloud);
// 设置需要计算SHOT的点云数据对应的法向量
shot_estimation.setInputNormals(cloud_normals);
//设置最近KDtree方法
shot_estimation.setSearchMethod(tree);
//创建保存SHOT特征信息的对象shot_features
pcl::PointCloud<pcl::SHOT352>::Ptr shot_features(new pcl::PointCloud<pcl::SHOT352>);
//设置搜索的邻域半径
shot_estimation.setRadiusSearch(0.08);
// 计算SHOT特征信息将结构保存到shot_features中
shot_estimation.compute(*shot_features);
// 输出索引为2000的SHOT特征信息
pcl::SHOT352 descriptor = (*shot_features)[2000];
std::cout << descriptor << std::endl;
}
int main(int argc, char* argv[])
{
PCLSHOTFeature();
std::cout << "Hello PCL World!" << std::endl;
std::system("pause");
return 0;
}结果：
至此完成第十六节PCL库点云特征之SHOT特征直方图学习，下一节我们将进入《PCL库中点云关键点》的学习。