PCL 1.15.1的common/common.h的深度解析

common.h源码（中文）

#pragma once

#ifdef __SSE__
#include <xmmintrin.h> // 用于 __m128 类型(SSE指令集)
#endif // ifdef __SSE__
#ifdef __AVX__
#include <immintrin.h> // 用于 __m256 类型(AVX指令集)
#endif // ifdef __AVX__

#include <pcl/point_cloud.h> // 点云数据结构
#include <pcl/PointIndices.h> // 点索引集合
namespace pcl { struct PCLPointCloud2; }

/**
* \\file pcl/common/common.h
* 定义所有PCL方法通用的标准C方法和C++类
* 本文件包含常用的几何计算、统计分析和点云处理工具函数
* \\ingroup common
*/

/*@{*/
namespace pcl
{
/** \\brief 计算两个3D向量之间的最小夹角(默认弧度制,可选角度制)
* \\param v1 第一个3D向量(用 \\a Eigen::Vector4f 表示,齐次坐标)
* \\param v2 第二个3D向量(用 \\a Eigen::Vector4f 表示,齐次坐标)
* \\param in_degree 确定返回角度是弧度还是角度(默认false为弧度)
* \\return v1和v2之间的夹角,单位为弧度或角度
* \\note 处理平行和反平行向量的舍入误差,确保数值稳定性
* \\note 适用场景:法向量夹角计算、方向判断等
* \\ingroup common
*/
inline double
getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree = false);

/** \\brief 计算两个3D向量之间的最小夹角(默认弧度制,可选角度制)
* \\param v1 第一个3D向量(用 \\a Eigen::Vector3f 表示)
* \\param v2 第二个3D向量(用 \\a Eigen::Vector3f 表示)
* \\param in_degree 确定返回角度是弧度还是角度(默认false为弧度)
* \\return v1和v2之间的夹角,单位为弧度或角度
* \\note 与上面的重载版本功能相同,但使用3维向量而非齐次坐标
* \\ingroup common
*/
inline double
getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree = false);

#ifdef __SSE__
/** \\brief 使用SSE指令同时计算四个值的近似反余弦值
*
* 使用的近似公式为 \\((1.59121552+x*(-0.15461442+x*0.05354897))*\\sqrt{0.89286965-0.89282669*x}+0.06681017+x*(-0.09402311+x*0.02708663)\\)
* 平均误差为0.00012弧度。此近似方法比其他acos近似更精确,但也使用更多运算。
* \\param x 四个浮点数,每个都应在[0; 1]范围内。不能大于1(acos在该处未定义)。
* 不应小于0,因为在负值处近似精度较低
* \\return 四个反余弦值,每个都在[0; pi/2]范围内
* \\note 利用SIMD指令并行计算,提升性能约4倍
* \\note 适用场景:批量角度计算、法向量匹配等高性能需求场景
* \\ingroup common
*/
inline __m128
acos_SSE (const __m128 &x);

/** \\brief 类似getAngle3D,但使用SSE指令并行计算四组向量夹角
*
* 行为类似 \\(\\min(getAngle3D(dot\\_product), \\pi-getAngle3D(dot\\_product))\\)
* 所有向量必须归一化(长度为1.0)
* 由于使用近似acos,结果可能略有误差
* \\param[in] x1 前四个向量的x分量
* \\param[in] y1 前四个向量的y分量
* \\param[in] z1 前四个向量的z分量
* \\param[in] x2 后四个向量的x分量
* \\param[in] y2 后四个向量的y分量
* \\param[in] z2 后四个向量的z分量
* \\return 四个夹角(弧度),范围在[0; pi/2]内
* \\note 返回锐角,即总是返回小于等于90度的角度
* \\note 适用场景:点云法向量匹配、特征描述子计算等需要大量角度计算的场景
* \\ingroup common
*/
inline __m128
getAcuteAngle3DSSE (const __m128 &x1, const __m128 &y1, const __m128 &z1, const __m128 &x2, const __m128 &y2, const __m128 &z2);
#endif // ifdef __SSE__

#ifdef __AVX__
/** \\brief 使用AVX指令同时计算八个值的近似反余弦值
*
* 使用的近似公式为 \\((1.59121552+x*(-0.15461442+x*0.05354897))*\\sqrt{0.89286965-0.89282669*x}+0.06681017+x*(-0.09402311+x*0.02708663)\\)
* 平均误差为0.00012弧度。此近似方法比其他acos近似更精确,但也使用更多运算。
* \\param x 八个浮点数,每个都应在[0; 1]范围内。不能大于1(acos在该处未定义)。
* 不应小于0,因为在负值处近似精度较低
* \\return 八个反余弦值,每个都在[0; pi/2]范围内
* \\note AVX指令集相比SSE可并行处理8个值,进一步提升性能
* \\note 需要CPU支持AVX指令集(Intel Sandy Bridge及更新架构)
* \\ingroup common
*/
inline __m256
acos_AVX (const __m256 &x);

/** \\brief 类似getAngle3D,但使用AVX指令并行计算八组向量夹角
*
* 行为类似 \\(\\min(getAngle3D(dot\\_product), \\pi-getAngle3D(dot\\_product))\\)
* 所有向量必须归一化(长度为1.0)
* 由于使用近似acos,结果可能略有误差
* \\param[in] x1 前八个向量的x分量
* \\param[in] y1 前八个向量的y分量
* \\param[in] z1 前八个向量的z分量
* \\param[in] x2 后八个向量的x分量
* \\param[in] y2 后八个向量的y分量
* \\param[in] z2 后八个向量的z分量
* \\return 八个夹角(弧度),范围在[0; pi/2]内
* \\note 性能比SSE版本提升约2倍,适合大规模点云处理
* \\ingroup common
*/
inline __m256
getAcuteAngle3DAVX (const __m256 &x1, const __m256 &y1, const __m256 &z1, const __m256 &x2, const __m256 &y2, const __m256 &z2);
#endif // ifdef __AVX__

/** \\brief 计算一组数值的均值和标准差
* \\param values 数值数组
* \\param mean 输出参数:分布的均值
* \\param stddev 输出参数:分布的标准差
* \\note 标准差反映数据的离散程度
* \\note 适用场景:点云特征统计、离群点检测等
* \\ingroup common
*/
inline void
getMeanStd (const std::vector<float> &values, double &mean, double &stddev);

/** \\brief 获取在给定边界框内的所有点的索引
* \\param cloud 点云数据
* \\param min_pt 边界框的最小坐标(左下后角点)
* \\param max_pt 边界框的最大坐标(右上前角点)
* \\param indices 输出参数:位于边界框内的点的索引集合
* \\note 边界框是轴对齐的(AABB),不考虑旋转
* \\note 适用场景:空间裁剪、感兴趣区域提取等
* \\ingroup common
*/
template <typename PointT> inline void
getPointsInBox (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt,
Eigen::Vector4f &max_pt, Indices &indices);

/** \\brief 获取点云中距离给定点最远的点
* \\param cloud 点云数据
* \\param pivot_pt 参考点,用于计算距离
* \\param max_pt 输出参数:cloud中距离pivot_pt最远的点
* \\note 使用欧氏距离计算
* \\note 适用场景:点云边界检测、最大范围计算等
* \\ingroup common
*/
template<typename PointT> inline void
getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt);

/** \\brief 获取点云子集中距离给定点最远的点
* \\param cloud 点云数据
* \\param indices 要使用的点索引向量(仅在这些点中搜索)
* \\param pivot_pt 参考点,用于计算距离
* \\param max_pt 输出参数:索引子集中距离pivot_pt最远的点
* \\note 相比全点云版本,此版本可在指定子集中搜索,提高效率
* \\ingroup common
*/
template<typename PointT> inline void
getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt);

/** \\brief 获取点云在三个维度(x-y-z)上的最小值和最大值,即轴对齐包围盒(AABB)
* \\param[in] cloud 点云数据
* \\param[out] min_pt 输出参数:最小边界点
* \\param[out] max_pt 输出参数:最大边界点
* \\note AABB是最简单的包围盒,边与坐标轴平行
* \\note 适用场景:碰撞检测、空间划分、可视化范围确定等
* \\ingroup common
*/
template <typename PointT> inline void
getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt);

/** \\brief 获取点云在三个维度(x-y-z)上的最小值和最大值,即轴对齐包围盒(AABB)
* \\param[in] cloud 点云数据
* \\param[out] min_pt 输出参数:最小边界点(Eigen向量格式)
* \\param[out] max_pt 输出参数:最大边界点(Eigen向量格式)
* \\note 使用Eigen::Vector4f格式,方便后续矩阵运算
* \\ingroup common
*/
template <typename PointT> inline void
getMinMax3D (const pcl::PointCloud<PointT> &cloud,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);

/** \\brief 获取点云子集在三个维度(x-y-z)上的最小值和最大值,即轴对齐包围盒(AABB)
* \\param[in] cloud 点云数据
* \\param[in] indices 要使用的点索引向量
* \\param[out] min_pt 输出参数:最小边界点
* \\param[out] max_pt 输出参数:最大边界点
* \\note 仅计算指定索引子集的包围盒,适合处理分割后的点云
* \\ingroup common
*/
template <typename PointT> inline void
getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);

/** \\brief 获取点云子集在三个维度(x-y-z)上的最小值和最大值,即轴对齐包围盒(AABB)
* \\param[in] cloud 点云数据
* \\param[in] indices PointIndices结构体,包含要使用的点索引
* \\param[out] min_pt 输出参数:最小边界点
* \\param[out] max_pt 输出参数:最大边界点
* \\note PointIndices是PCL的标准索引容器,常用于点云分割结果
* \\ingroup common
*/
template <typename PointT> inline void
getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt);

/** \\brief 计算由三个点pa、pb、pc构成的三角形的外接圆半径
* \\param pa 第一个点
* \\param pb 第二个点
* \\param pc 第三个点
* \\return 外接圆的半径
* \\note 外接圆是通过三角形三个顶点的圆
* \\note 适用场景:三角网格质量评估、Delaunay三角剖分等
* \\ingroup common
*/
template <typename PointT> inline double
getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc);

/** \\brief 获取点直方图(多维特征)的最小值和最大值
* \\param histogram 表示多维直方图的点
* \\param len 直方图的长度(维度数)
* \\param min_p 输出参数:最小值
* \\param max_p 输出参数:最大值
* \\note 用于特征归一化、特征可视化等
* \\note 适用场景:PFH、FPFH等特征描述子的统计分析
* \\ingroup common
*/
template <typename PointT> inline void
getMinMax (const PointT &histogram, int len, float &min_p, float &max_p);

/** \\brief 计算由点云定义的多边形的面积
* \\param polygon 包含多边形顶点的点云。顶点按逆时针顺序存储
* \\return 多边形面积
* \\note 假设多边形是平面的且顶点共面
* \\note 适用场景:表面积计算、平面区域测量等
* \\ingroup common
*/
template<typename PointT> inline float
calculatePolygonArea (const pcl::PointCloud<PointT> &polygon);

/** \\brief 获取点云中某个点的多维直方图字段的最小值和最大值
* \\param cloud 包含多维直方图的点云
* \\param idx 需要计算最小/最大值的直方图对应的点索引
* \\param field_name 包含多维直方图的字段名称
* \\param min_p 输出参数:最小值
* \\param max_p 输出参数:最大值
* \\note 适用于PCLPointCloud2格式的通用点云数据
* \\note 可处理任意命名的特征字段
* \\ingroup common
*/
PCL_EXPORTS void
getMinMax (const pcl::PCLPointCloud2 &cloud, int idx, const std::string &field_name,
float &min_p, float &max_p);

/** \\brief 计算一组数值的均值和标准差
* \\param values 数值数组
* \\param mean 输出参数:分布的均值
* \\param stddev 输出参数:分布的标准差
* \\note 与getMeanStd功能相同,可能是为了命名一致性提供的重载版本
* \\ingroup common
*/
PCL_EXPORTS void
getMeanStdDev (const std::vector<float> &values, double &mean, double &stddev);

/** \\brief 快速计算数值列表的中位数。如果数值个数为偶数,取中间两个值的均值
* 此函数可以这样使用:
* \\code{.cpp}
* std::vector<double> vector{1.0, 25.0, 9.0, 4.0, 16.0};
* const double median = pcl::computeMedian (vector.begin (), vector.end (), static_cast<double(*)(double)>(std::sqrt)); // = 3
* \\endcode
* \\param[in,out] begin,end 标记值范围开始和结束的迭代器。注意:这些值会被重新排序!
* \\param[in] f lambda表达式、函数指针或类似对象,隐式应用于所有值(在计算中位数之前)。实际上,它会被延迟计算(最多两次),因此不应改变排序顺序(例如允许使用单调函数如sqrt)
* \\return 中位数
* \\note 使用nth_element算法,时间复杂度O(n),比完全排序更快
* \\note 会修改原始数据顺序!如需保留原数据,请先复制
* \\note 适用场景:稳健统计、离群点检测等
* \\ingroup common
*/
template<typename IteratorT, typename Functor> inline auto
computeMedian (IteratorT begin, IteratorT end, Functor f) noexcept ->
#if __cpp_lib_is_invocable
std::invoke_result_t<Functor, decltype(*begin)>
#else
std::result_of_t<Functor(decltype(*begin))>
#endif
{
const std::size_t size = std::distance(begin, end);
const std::size_t mid = size/2;
if (size%2==0)
{ // 偶数个值:取中间两个的平均
std::nth_element (begin, begin + (mid-1), end);
return (f(begin[mid-1]) + f(*(std::min_element (begin + mid, end)))) / 2.0;
}
else
{ // 奇数个值:直接取中间值
std::nth_element (begin, begin + mid, end);
return f(begin[mid]);
}
}

/** \\brief 计算数值列表的中位数(快速)。详见另一个重载函数的说明
* \\note 不应用任何变换函数,直接计算原始值的中位数
*/
template<typename IteratorT> inline auto
computeMedian (IteratorT begin, IteratorT end) noexcept -> typename std::iterator_traits<IteratorT>::value_type
{
return computeMedian (begin, end, [](const auto& x){return x;});
}
}
/*@}*/
#include <pcl/common/impl/common.hpp>

common.hpp源码（中文）

#ifndef PCL_COMMON_IMPL_H_
#define PCL_COMMON_IMPL_H_

#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <limits>

//////////////////////////////////////////////////////////////////////////////////////////////
inline double
pcl::getAngle3D (const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree)
{
// 计算实际夹角
// 先归一化向量,再计算点积得到cos值
double rad = v1.normalized ().dot (v2.normalized ());
// 限制rad在[-1, 1]范围内,防止浮点误差导致acos参数越界
if (rad < -1.0)
rad = -1.0;
else if (rad > 1.0)
rad = 1.0;
// 根据参数选择返回弧度或角度
return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
}

inline double
pcl::getAngle3D (const Eigen::Vector3f &v1, const Eigen::Vector3f &v2, const bool in_degree)
{
// 计算实际夹角(3D向量版本)
// 算法与Vector4f版本相同
double rad = v1.normalized ().dot (v2.normalized ());
if (rad < -1.0)
rad = -1.0;
else if (rad > 1.0)
rad = 1.0;
return (in_degree ? std::acos (rad) * 180.0 / M_PI : std::acos (rad));
}

#ifdef __SSE__
inline __m128
pcl::acos_SSE (const __m128 &x)
{
/*
以下Python代码用于生成近似系数:
import math, numpy, scipy.optimize
def get_error(S):
err_sum=0.0
for x in numpy.arange(0.0, 1.0, 0.0025):
if (S[3]+S[4]*x)<0.0:
err_sum+=10.0
else:
err_sum+=((S[0]+x*(S[1]+x*S[2]))*numpy.sqrt(S[3]+S[4]*x)+S[5]+x*(S[6]+x*S[7])-math.acos(x))**2.0
return err_sum/400.0

print(scipy.optimize.minimize(fun=get_error, x0=[1.57, 0.0, 0.0, 1.0, -1.0, 0.0, 0.0, 0.0], method='Nelder-Mead', options={'maxiter':42000, 'maxfev':42000, 'disp':True, 'xatol':1e-6, 'fatol':1e-6}))

该近似公式使用多项式和平方根的组合来逼近acos函数
乘法项: (1.59121552 + x*(-0.15461442 + x*0.05354897))
平方根项: sqrt(0.89286965 – 0.89282669*x)
加法项: 0.06681017 + x*(-0.09402311 + x*0.02708663)
最终结果 = 乘法项 * 平方根项 + 加法项
*/
const __m128 mul_term = _mm_add_ps (_mm_set1_ps (1.59121552f), _mm_mul_ps (x, _mm_add_ps (_mm_set1_ps (-0.15461442f), _mm_mul_ps (x, _mm_set1_ps (0.05354897f)))));
const __m128 add_term = _mm_add_ps (_mm_set1_ps (0.06681017f), _mm_mul_ps (x, _mm_add_ps (_mm_set1_ps (-0.09402311f), _mm_mul_ps (x, _mm_set1_ps (0.02708663f)))));
return _mm_add_ps (_mm_mul_ps (mul_term, _mm_sqrt_ps (_mm_add_ps (_mm_set1_ps (0.89286965f), _mm_mul_ps (_mm_set1_ps (-0.89282669f), x)))), add_term);
}

inline __m128
pcl::getAcuteAngle3DSSE (const __m128 &x1, const __m128 &y1, const __m128 &z1, const __m128 &x2, const __m128 &y2, const __m128 &z2)
{
// 计算四组向量的点积: dot = x1*x2 + y1*y2 + z1*z2
const __m128 dot_product = _mm_add_ps (_mm_add_ps (_mm_mul_ps (x1, x2), _mm_mul_ps (y1, y2)), _mm_mul_ps (z1, z2));
// andnot函数实现取绝对值操作:移除符号位
// -0.0f(负零)表示所有位都是0,只有符号位是1
// 通过andnot操作移除符号位,得到点积的绝对值
// min操作确保值不超过1.0(防止数值误差)
// 最后计算acos得到锐角
return acos_SSE (_mm_min_ps (_mm_set1_ps (1.0f), _mm_andnot_ps (_mm_set1_ps (-0.0f), dot_product)));
}
#endif // ifdef __SSE__

#ifdef __AVX__
inline __m256
pcl::acos_AVX (const __m256 &x)
{
// AVX版本的acos近似计算,与SSE版本算法相同,但一次处理8个值
// 使用相同的多项式近似公式
const __m256 mul_term = _mm256_add_ps (_mm256_set1_ps (1.59121552f), _mm256_mul_ps (x, _mm256_add_ps (_mm256_set1_ps (-0.15461442f), _mm256_mul_ps (x, _mm256_set1_ps (0.05354897f)))));
const __m256 add_term = _mm256_add_ps (_mm256_set1_ps (0.06681017f), _mm256_mul_ps (x, _mm256_add_ps (_mm256_set1_ps (-0.09402311f), _mm256_mul_ps (x, _mm256_set1_ps (0.02708663f)))));
return _mm256_add_ps (_mm256_mul_ps (mul_term, _mm256_sqrt_ps (_mm256_add_ps (_mm256_set1_ps (0.89286965f), _mm256_mul_ps (_mm256_set1_ps (-0.89282669f), x)))), add_term);
}

inline __m256
pcl::getAcuteAngle3DAVX (const __m256 &x1, const __m256 &y1, const __m256 &z1, const __m256 &x2, const __m256 &y2, const __m256 &z2)
{
// 计算八组向量的点积
const __m256 dot_product = _mm256_add_ps (_mm256_add_ps (_mm256_mul_ps (x1, x2), _mm256_mul_ps (y1, y2)), _mm256_mul_ps (z1, z2));
// andnot函数实现取绝对值操作:移除符号位
// -0.0f(负零)表示所有位都是0,只有符号位是1
// AVX版本一次处理8组向量,性能是SSE的两倍
return acos_AVX (_mm256_min_ps (_mm256_set1_ps (1.0f), _mm256_andnot_ps (_mm256_set1_ps (-0.0f), dot_product)));
}
#endif // ifdef __AVX__

//////////////////////////////////////////////////////////////////////////////////////////////
inline void
pcl::getMeanStd (const std::vector<float> &values, double &mean, double &stddev)
{
// 当输入数组为空时抛出异常
if (values.empty ())
{
PCL_THROW_EXCEPTION (BadArgumentException, "Input array must have at least 1 element.");
}

// 当数组只有一个元素时,均值就是该元素本身,标准差为0
if (values.size () == 1)
{
mean = values.at (0);
stddev = 0;
return;
}

double sum = 0, sq_sum = 0;

// 计算总和与平方和
for (const float &value : values)
{
sum += value;
sq_sum += value * value;
}
// 计算均值
mean = sum / static_cast<double>(values.size ());
// 计算方差: Var = E[X^2] – (E[X])^2
// 使用贝塞尔校正(除以n-1而非n)得到无偏估计
double variance = (sq_sum – sum * sum / static_cast<double>(values.size ())) / (static_cast<double>(values.size ()) – 1);
// 标准差是方差的平方根
stddev = sqrt (variance);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline void
pcl::getPointsInBox (const pcl::PointCloud<PointT> &cloud,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt,
Indices &indices)
{
// 预分配索引数组大小(最坏情况是所有点都在盒内)
indices.resize (cloud.size ());
int l = 0;

// 如果数据是稠密的(没有NaN),无需检查无效值
if (cloud.is_dense)
{
for (std::size_t i = 0; i < cloud.size (); ++i)
{
// 检查点是否在边界内:先检查是否小于最小边界
if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
continue;
// 再检查是否大于最大边界
if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
continue;
// 点在盒内,记录其索引
indices[l++] = static_cast<int>(i);
}
}
// 可能存在NaN或Inf值 => 需要检查
else
{
for (std::size_t i = 0; i < cloud.size (); ++i)
{
// 检查点是否无效(NaN或Inf)
if (!std::isfinite (cloud[i].x) ||
!std::isfinite (cloud[i].y) ||
!std::isfinite (cloud[i].z))
continue;
// 检查点是否在边界内
if (cloud[i].x < min_pt[0] || cloud[i].y < min_pt[1] || cloud[i].z < min_pt[2])
continue;
if (cloud[i].x > max_pt[0] || cloud[i].y > max_pt[1] || cloud[i].z > max_pt[2])
continue;
indices[l++] = static_cast<int>(i);
}
}
// 调整索引数组大小为实际点数
indices.resize (l);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
// 初始化最大距离为最小可能值
float max_dist = std::numeric_limits<float>::lowest();
int max_idx = -1;
float dist;
// 提取参考点的3D坐标
const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();

// 如果数据是稠密的,无需检查NaN
if (cloud.is_dense)
{
for (std::size_t i = 0; i < cloud.size (); ++i)
{
// 获取当前点的3D坐标映射
pcl::Vector3fMapConst pt = cloud[i].getVector3fMap ();
// 计算欧氏距离
dist = (pivot_pt3 – pt).norm ();
// 更新最大距离和对应索引
if (dist > max_dist)
{
max_idx = static_cast<int>(i);
max_dist = dist;
}
}
}
// 可能存在NaN或Inf值 => 需要检查
else
{
for (std::size_t i = 0; i < cloud.size (); ++i)
{
// 检查点是否无效
if (!std::isfinite (cloud[i].x) || !std::isfinite (cloud[i].y) || !std::isfinite (cloud[i].z))
continue;
pcl::Vector3fMapConst pt = cloud[i].getVector3fMap ();
dist = (pivot_pt3 – pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int>(i);
max_dist = dist;
}
}
}

// 如果找到有效的最远点,返回其坐标;否则返回NaN
if(max_idx != -1)
max_pt = cloud[max_idx].getVector4fMap ();
else
max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> inline void
pcl::getMaxDistance (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
const Eigen::Vector4f &pivot_pt, Eigen::Vector4f &max_pt)
{
// 在指定索引子集中查找最远点
float max_dist = std::numeric_limits<float>::lowest();
int max_idx = -1;
float dist;
const Eigen::Vector3f pivot_pt3 = pivot_pt.head<3> ();

// 如果数据是稠密的,无需检查NaN
if (cloud.is_dense)
{
for (std::size_t i = 0; i < indices.size (); ++i)
{
pcl::Vector3fMapConst pt = cloud[indices[i]].getVector3fMap ();
dist = (pivot_pt3 – pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int> (i);
max_dist = dist;
}
}
}
// 可能存在NaN或Inf值 => 需要检查
else
{
for (std::size_t i = 0; i < indices.size (); ++i)
{
// 检查点是否无效
if (!std::isfinite (cloud[indices[i]].x) || !std::isfinite (cloud[indices[i]].y)
||
!std::isfinite (cloud[indices[i]].z))
continue;

pcl::Vector3fMapConst pt = cloud[indices[i]].getVector3fMap ();
dist = (pivot_pt3 – pt).norm ();
if (dist > max_dist)
{
max_idx = static_cast<int> (i);
max_dist = dist;
}
}
}

// 注意:max_idx是indices数组中的索引,需要通过indices[max_idx]获取实际点索引
if(max_idx != -1)
max_pt = cloud[indices[max_idx]].getVector4fMap ();
else
max_pt = Eigen::Vector4f(std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN(),std::numeric_limits<float>::quiet_NaN());
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, PointT &min_pt, PointT &max_pt)
{
// 内部使用Eigen::Vector4f版本计算,然后转换为PointT类型
Eigen::Vector4f min_p, max_p;
pcl::getMinMax3D (cloud, min_p, max_p);
// 将结果复制到输出点的x,y,z分量
min_pt.x = min_p[0]; min_pt.y = min_p[1]; min_pt.z = min_p[2];
max_pt.x = max_p[0]; max_pt.y = max_p[1]; max_pt.z = max_p[2];
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
// 初始化最小值为float最大值,最大值为float最小值
min_pt.setConstant (std::numeric_limits<float>::max());
max_pt.setConstant (std::numeric_limits<float>::lowest());

// 如果数据是稠密的,无需检查NaN
if (cloud.is_dense)
{
for (const auto& point: cloud.points)
{
// 获取点的4D向量表示(x,y,z,1)
const pcl::Vector4fMapConst pt = point.getVector4fMap ();
// cwiseMin/cwiseMax:逐元素比较取最小/最大值
min_pt = min_pt.cwiseMin (pt);
max_pt = max_pt.cwiseMax (pt);
}
}
// 可能存在NaN或Inf值 => 需要检查
else
{
for (const auto& point: cloud.points)
{
// 检查点是否无效
if (!std::isfinite (point.x) ||
!std::isfinite (point.y) ||
!std::isfinite (point.z))
continue;
const pcl::Vector4fMapConst pt = point.getVector4fMap ();
min_pt = min_pt.cwiseMin (pt);
max_pt = max_pt.cwiseMax (pt);
}
}
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
// PointIndices是PCL标准索引容器,提取其内部的indices向量调用重载版本
pcl::getMinMax3D (cloud, indices.indices, min_pt, max_pt);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline void
pcl::getMinMax3D (const pcl::PointCloud<PointT> &cloud, const Indices &indices,
Eigen::Vector4f &min_pt, Eigen::Vector4f &max_pt)
{
// 初始化边界值
min_pt.setConstant (std::numeric_limits<float>::max());
max_pt.setConstant (std::numeric_limits<float>::lowest());

// 如果数据是稠密的,无需检查NaN
if (cloud.is_dense)
{
for (const auto &index : indices)
{
const pcl::Vector4fMapConst pt = cloud[index].getVector4fMap ();
min_pt = min_pt.cwiseMin (pt);
max_pt = max_pt.cwiseMax (pt);
}
}
// 可能存在NaN或Inf值 => 需要检查
else
{
for (const auto &index : indices)
{
// 检查点是否无效
if (!std::isfinite (cloud[index].x) ||
!std::isfinite (cloud[index].y) ||
!std::isfinite (cloud[index].z))
continue;
const pcl::Vector4fMapConst pt = cloud[index].getVector4fMap ();
min_pt = min_pt.cwiseMin (pt);
max_pt = max_pt.cwiseMax (pt);
}
}
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline double
pcl::getCircumcircleRadius (const PointT &pa, const PointT &pb, const PointT &pc)
{
// 将三个点转换为Eigen向量
Eigen::Vector4f p1 (pa.x, pa.y, pa.z, 0);
Eigen::Vector4f p2 (pb.x, pb.y, pb.z, 0);
Eigen::Vector4f p3 (pc.x, pc.y, pc.z, 0);

// 计算三角形三条边的长度
double p2p1 = (p2 – p1).norm (), p3p2 = (p3 – p2).norm (), p1p3 = (p1 – p3).norm ();
// 使用海伦公式计算三角形面积
// (https://en.wikipedia.org/wiki/Heron's_formula)
// 半周长 s = (a + b + c) / 2
double semiperimeter = (p2p1 + p3p2 + p1p3) / 2.0;
// 面积 A = sqrt(s * (s-a) * (s-b) * (s-c))
double area = sqrt (semiperimeter * (semiperimeter – p2p1) * (semiperimeter – p3p2) * (semiperimeter – p1p3));
// 计算外接圆半径:R = (a * b * c) / (4 * A)
// 这个公式来自三角形面积与外接圆半径的关系
return ((p2p1 * p3p2 * p1p3) / (4.0 * area));
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline void
pcl::getMinMax (const PointT &histogram, int len, float &min_p, float &max_p)
{
// 初始化最小值为float最大值,最大值为float最小值
min_p = std::numeric_limits<float>::max();
max_p = std::numeric_limits<float>::lowest();

// 遍历直方图的所有维度
for (int i = 0; i < len; ++i)
{
// 使用三元运算符更新最小值和最大值
min_p = (histogram[i] > min_p) ? min_p : histogram[i];
max_p = (histogram[i] < max_p) ? max_p : histogram[i];
}
}

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> inline float
pcl::calculatePolygonArea (const pcl::PointCloud<PointT> &polygon)
{
float area = 0.0f;
int num_points = polygon.size ();
Eigen::Vector3f va,vb,res;

// 初始化结果向量为零
res(0) = res(1) = res(2) = 0.0f;
// 使用叉积法计算多边形面积
// 原理:将多边形分解为从原点出发到各边的三角形
// 每条边对应的三角形面积向量通过叉积计算
for (int i = 0; i < num_points; ++i)
{
// 获取当前点和下一个点(首尾相连)
int j = (i + 1) % num_points;
va = polygon[i].getVector3fMap ();
vb = polygon[j].getVector3fMap ();
// 累加叉积:va × vb,其模长的一半是对应三角形面积
res += va.cross (vb);
}
// 计算累加叉积向量的模长
area = res.norm ();
// 返回面积的一半(因为叉积模长是平行四边形面积)
return (area*0.5);
}

#endif //#ifndef PCL_COMMON_IMPL_H_

PCL 1.15.1 common.h 深度解析

这个文件是 PCL 库中底层中的底层。它不依赖于复杂的数据结构（如 KdTree 或 Octree），而是提供了最基础的数学、几何和统计工具。这些函数被广泛应用于 pcl_filters、pcl_features 和 pcl_segmentation 等高层模块中。

1. getAngle3D：3D 向量夹角计算

• 含义与作用：计算两个 3D 向量之间的最小夹角。它支持 Eigen::Vector4f（齐次坐标）和 Eigen::Vector3f 两种输入。

• 技术细节：内部通过点积（Dot Product）除以模长的乘积，再取 acos 得到。PCL 在实现时做了数值稳定性处理，防止因浮点数精度问题导致点积结果略微超出 范围而产生 NaN。

• 如何服务其他方法：这是法向量分析的基础。在 pcl::NormalEstimation 中判断法向量一致性，或在 SACSegmentation（采样一致性分割）中通过法线约束过滤平面时，都会频繁调用此函数。

代码示例：

#include <pcl/common/common.h>
#include <Eigen/Dense>
#include <iostream>

void exampleGetAngle() {
Eigen::Vector4f v1(1.0, 0.0, 0.0, 0.0);
Eigen::Vector4f v2(0.0, 1.0, 0.0, 0.0);

// 计算弧度 (默认)
double angle_rad = pcl::getAngle3D(v1, v2);
// 计算角度
double angle_deg = pcl::getAngle3D(v1, v2, true);

std::cout << "Angle: " << angle_rad << " rad, " << angle_deg << " deg" << std::endl;
}

2. acos_SSE / acos_AVX：SIMD 加速的反余弦

• 含义与作用：利用 CPU 的单指令多数据流（SIMD）指令集，一次性计算 4 个（SSE）或 8 个（AVX）浮点数的近似反余弦值。

• 技术细节：它不直接调用标准库的 std::acos，而是使用了一个高阶多项式拟合公式。虽然会有 弧度左右的误差，但换取了近 4-8 倍的性能提升。

• 如何服务其他方法：在大规模点云特征提取（如 FPFH 或 SHOT 描述子）中，需要计算数百万次角度。如果没有这种硬件级加速，实时性将无法保障。

3. getAcuteAngle3DSSE / getAcuteAngle3DAVX：并行锐角计算

• 含义与作用：这是 getAngle3D 的批量硬件加速版。它计算两组向量之间的夹角，并强制返回锐角（）。

• 技术细节：要求输入的向量必须已经归一化（Normalized）。它直接操作 SSE/AVX 寄存器，适用于高性能几何计算管线。

• 如何服务其他方法：主要用于特征描述子的计算管线，在处理数千万规模的点云匹配时，它是降低延迟的核心。

4. getMeanStd / getMeanStdDev：均值与标准差

• 含义与作用：计算一个 std::vector<float> 数组中数值的算术平均值和样本标准差。

• 技术细节：标准差反映了数据的离散程度。PCL 在这里使用了标准的统计学公式，计算过程中注意了数值累加的稳定性。

• 如何服务其他方法：它是 离群点剔除（StatisticalOutlierRemoval） 的核心。通过计算每个点到其邻域点的平均距离分布，利用均值和标准差确定“标准差倍数阈值”，从而识别噪声点。

代码示例：

void exampleStats() {
std::vector<float> distances = {0.1, 0.2, 0.15, 0.8, 0.12}; // 0.8 可能是噪声
double mean, stddev;
pcl::getMeanStd(distances, mean, stddev);

// 如果 distance > mean + 2*stddev，则认为是噪声
std::cout << "Mean: " << mean << ", StdDev: " << stddev << std::endl;
}

5. getPointsInBox：轴对齐包围盒区域提取

• 含义与作用：从给定的点云中，找出所有位于指定 AABB（轴对齐包围盒）范围内的点的索引。

• 技术细节：这是一种简单的线性搜索方法。它遍历点云，检查每个点的 是否都在 区间内。

• 如何服务其他方法：在不需要构建复杂索引（如 KdTree）的情况下，快速从场景中裁剪出一个立方体区域（ROI 提取）。它是 pcl::CropBox 滤波器的底层实现逻辑。

代码示例：

void exampleBoxQuery(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
Eigen::Vector4f min_pt(-1.0, -1.0, -1.0, 1.0);
Eigen::Vector4f max_pt(1.0, 1.0, 1.0, 1.0);
pcl::Indices indices;

pcl::getPointsInBox(*cloud, min_pt, max_pt, indices);
std::cout << "Found " << indices.size() << " points in box." << std::endl;
}

6. getMaxDistance：搜索最远点

• 含义与作用：在一个点云或点云子集中，找到距离给定参考点（pivot_pt）最远的点。

• 技术细节：通过计算每个点与参考点的欧氏距离平方（避免开方运算以提升速度）并维护一个最大值来实现。

• 如何服务其他方法：用于计算点云的跨度（Extent）。在计算点云的近似直径、外接球半径或者进行数据归一化（将点云缩放到单位球内）时非常有用。

7. getMinMax3D：轴对齐包围盒（AABB）计算

• 含义与作用：这是 PCL 中最常用的工具函数之一。它计算点云在 三个维度上的最小值和最大值。

• 技术细节：该函数有多个重载版本，支持全点云输入、带索引输入、以及 PointIndices 结构输入。

• 如何服务其他方法：

• VoxelGrid 降采样：必须先调用此函数确定体素网格的起始坐标和网格数量。

• 可视化：用于初始化相机的视角，确保点云在屏幕中心。

• 坐标转换：在进行点云归一化或平移到原点时作为基准。

代码示例：

void exampleMinMax(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
pcl::PointXYZ min_p, max_p;
pcl::getMinMax3D(*cloud, min_p, max_p);

std::cout << "Bounding Box: "
<< "X: [" << min_p.x << ", " << max_p.x << "], "
<< "Y: [" << min_p.y << ", " << max_p.y << "], "
<< "Z: [" << min_p.z << ", " << max_p.z << "]" << std::endl;
}

8. getCircumcircleRadius：三角形外接圆半径

• 含义与作用：计算由 3 个点 构成的三角形的外接圆半径 。

• 技术细节：计算公式基于三角形三边长 和面积 ：。PCL 的实现考虑了退化情况（如三点共线）。

• 如何服务其他方法：它是**曲面重建（Surface Reconstruction）**算法的“质检员”。在 pcl::GreedyProjectionTriangulation（贪婪投影三角化）中，会根据外接圆半径来判断一个三角形是否“过大”或“过扁”，从而剔除不合理的网格。

代码示例：

#include <pcl/common/common.h>
#include <pcl/point_types.h>

void exampleCircumradius() {
pcl::PointXYZ p1(0, 0, 0), p2(1, 0, 0), p3(0, 1, 0);
double radius = pcl::getCircumcircleRadius(p1, p2, p3);
// 对于直角边为1的等腰直角三角形，外接圆半径应为 sqrt(2)/2 ≈ 0.707
std::cout << "Circumcircle Radius: " << radius << std::endl;
}

9. getMinMax (Histogram)：直方图特征统计

• 含义与作用：获取单个点中多维直方图字段的最小值和最大值。

• 技术细节：这里的 PointT 通常是一个特征描述子（如 pcl::FPFHSignature33）。函数遍历该点内部的 histogram 数组。

• 如何服务其他方法：

• 特征归一化：在进行特征匹配前，将不同量级的特征缩放到 空间。

• 可视化：在 pcl::PCLVisualizer 中绘制特征直方图时，用于确定 Y 轴的显示范围。

10. calculatePolygonArea：多边形面积计算

• 含义与作用：计算一个三维空间中平面多边形的面积。

• 技术细节：算法基于**测量员公式（Shoelace Formula）**的 3D 泛化版。它假设输入的点云按逆时针或顺时针顺序排列，并且所有点大致共面。

• 如何服务其他方法：用于平面分割后的尺寸评估。例如，在室内机器人导航中，通过 pcl::SACSegmentation 提取出地面和墙面后，调用此函数计算它们的实际物理面积，从而过滤掉过小的碎片面。

代码示例：

void exampleArea() {
pcl::PointCloud<pcl::PointXYZ> rect;
rect.push_back(pcl::PointXYZ(0,0,0));
rect.push_back(pcl::PointXYZ(2,0,0));
rect.push_back(pcl::PointXYZ(2,1,0));
rect.push_back(pcl::PointXYZ(0,1,0));

float area = pcl::calculatePolygonArea(rect);
std::cout << "Polygon Area: " << area << std::endl; // 输出应为 2.0
}

11. getMinMax (PCLPointCloud2)：通用数据字段极值

• 含义与作用：在不确定点云具体类型的情况下，通过字符串字段名（如 "intensity"、"curvature"）获取指定点在特定字段上的极值。

• 技术细节：操作的是 pcl::PCLPointCloud2 这种“二进制大对象”格式。它通过 field.offset 动态定位内存地址。

• 如何服务其他方法：主要服务于 文件 I/O 和 预处理。当你刚从 PCD 文件加载数据，还没决定将其转换为哪种模板类型时，可以用它快速检查属性范围（例如检查反射强度强度是否在正常区间）。

12. getMeanStdDev：统计重载版本

• 含义与作用：与 getMeanStd 功能完全一致，计算 vector 的均值和标准差。

• 技术细节：PCL 这里使用了 PCL_EXPORTS 宏，通常是为了跨 DLL 调用的符号导出，确保在 Windows 等平台上的兼容性。

13. computeMedian (Functor 版本)：高性能稳健统计

• 含义与作用：快速计算一组数据的中位数，并允许在计算过程中应用一个映射函数（Functor）。

• 技术细节：极其重要！它没有使用全排序（），而是使用了 std::nth_element。这是一种基于快速选择（Quickselect）的算法，平均复杂度为 。

  • 注意：此操作会修改输入容器的元素顺序。

• 如何服务其他方法：

• 稳健估计（LMedS）：在最小中值乘方（Least Median of Squares）算法中，用于寻找最能代表整体趋势的参数，对离群点（Outliers）极度不敏感。

• 中值滤波：在 pcl::MedianFilter 中用于去除脉冲噪声。

代码示例：

void exampleMedian() {
std::vector<double> data = {10.0, 1.0, 5.0, 2.0, 8.0};
// 计算平方根后的中位数
auto sqrt_func = [](double x) { return std::sqrt(x); };
double med = pcl::computeMedian(data.begin(), data.end(), sqrt_func);

std::cout << "Median of sqrt(data): " << med << std::endl;
}

14. computeMedian (标准版本)：快速中位数

• 含义与作用：上述函数的简化版，直接计算原始数据列的中位数。

• 使用场景：当点云中的某个属性（如 Z 轴高度）包含极端噪声点时，使用中位数而非平均值来确定平面高度，可以有效避免被噪声带偏。

总结：如何高效使用 common.h

common.h 就像是 PCL 算法大厦的砖块，虽然简单，但质量直接决定了上层算法（如 SLAM、语义分割）的稳定性和效率。