LFSH 描述子的特征组成
设 p 为待描述点，以 p 为球心，给定半径 R 计算出其球形邻域，LFSH 描述子由此球形邻域中的各点计算得出。
LFSH 描述子由三个特征组成：
① 局部深度
p_i 为待描述点，n_i 为 p_i 的法线，平面 L 垂直于 n_i，并且与球形邻域相切 。将球形邻域中的点投影至平面 L，其中原点和投影点之间的距离 即为该点的局部深度。
② 法线偏角
法线偏角即待描述点的法线与其球形邻域内各点法线的夹角。
③ 点密度
上述的投影点 中，以待描述点的投影为圆心，划分若干半径，统计在每个环形区域内的点。
特征结合
三个特征对应三个直方图，直方图的 bin size 设置如下：局部深度8-15，法线夹角10-17，点密度4-7。文献中采取的 bin size 分别为10、15、5。
局部深度： 将每个点与其投影点的距离排序，设最大距离和最小距离之差为 d，则将 d 划分为 10 份，即 min ~ min + (1/10)d、min + (1/10)d ~ min + (2/10)d...，分别统计在10个距离中的点的个数。
法线夹角： 将计算出的每个点与待描述点的法线夹角进行排序，计算最大角度和最小角度之差，将此差值划分 15 份，同样分别统计在每个区间的点的个数。
点密度： 在投影点中，计算与待描述点的投影最远的距离，将此距离划分为5份，同样分别统计在各个距离内的点的个数。
将上述统计值直接结合，得到一个 bin size = 30 的直方图，此即为 LFSH 描述符。
代码实现
LFSH.h
cpp
#ifndef LFSH_H
#define LFSH_H
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <vector>
class LFSH
{
public:
LFSH(const pcl::PointXYZ& p, double r, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int N1, int N2, int N3, double normalR);
const pcl::PointCloud<pcl::PointXYZ>::Ptr& getCloud() { return m_cloud; }
const std::vector<double>& getDepth() { return m_depth; }
const std::vector<double>& getDegress() { return m_degress; }
const std::vector<int>& getDepthHistogram() { return m_depth_histogram; }
const std::vector<int>& getDegressHistogram() { return m_degress_histogram; }
const std::vector<int>& getDenistyHistogram() { return m_denisty_histogram; }
private:
pcl::PointXYZ m_P; // 待描述的点
double m_R; // 以 m_P 为圆心的球体的半径
pcl::PointCloud<pcl::PointXYZ>::Ptr m_SourceCloud; // 原始点云数据
pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud; // 以 m_P 为圆心的球体邻域
pcl::PointCloud<pcl::Normal>::Ptr m_normals; // 球体邻域内每个点的法线
std::vector<double> m_depth; // 局部深度
std::vector<int> m_depth_histogram; // 局部深度直方图
std::vector<double> m_degress; // 法线夹角
std::vector<int> m_degress_histogram; // 法线夹角直方图
std::vector<int> m_denisty_histogram; // 密度直方图
int m_N1, m_N2, m_N3; // 三个子直方图的 bin size
double m_normal_R; // 计算法线时使用的半径
};
#endif // LFSH_HLFSH.cpp
cpp
#define _CRT_SECURE_NO_WARNINGS
#include "lfsh.h"
#include <set>
#include <pcl/octree/octree_search.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <tuple>
/**
* @brief 返回给定点、给定半径的球体中点的索引
* @param r 球体半径
* @param p 给定球心
* @param cloud 操作的点云
* @return 球体中点的索引
*/
std::set<int> sphereSearch(double r, pcl::PointXYZ p, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
// 创建octree对象用于点云的搜索
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(0.5);
octree.setInputCloud(cloud);
octree.addPointsFromInputCloud();
// 存储球体内的点索引
std::vector<int> pointIdx;
std::vector<float> pointRadiusSquaredDistance;
// 用set存放球体内点的索引，提高查找效率
std::set<int> sphereIndices;
// 执行半径搜索
if (octree.radiusSearch(p, r, pointIdx, pointRadiusSquaredDistance) > 0)
{
sphereIndices.insert(pointIdx.begin(), pointIdx.end());
}
return sphereIndices;
}
/**
* @brief 提取球体邻域中的点
* @param sphereIndices 球体内点的索引
* @param cloud 原始点云
* @return 提取结果
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr sphereCloud(std::set<int> sphereIndices,
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr resultCloud(new pcl::PointCloud<pcl::PointXYZ>);
for (const auto& idx : sphereIndices)
{
resultCloud->push_back(cloud->points[idx]);
}
// 更新点云信息
resultCloud->width = resultCloud->points.size();
resultCloud->height = 1;
return resultCloud;
}
// 定义一个函数来计算与给定法线垂直并与球体相切的平面方程
std::vector<double> calculateTangentPlane(const pcl::PointXYZ& point, const pcl::Normal& normal, float radius)
{
// 计算法线的长度
float normal_length = std::sqrt(normal.normal_x * normal.normal_x +
normal.normal_y * normal.normal_y +
normal.normal_z * normal.normal_z);
// 计算切点坐标 Q(x1, y1, z1)
float x1 = point.x + radius * normal.normal_x / normal_length;
float y1 = point.y + radius * normal.normal_y / normal_length;
float z1 = point.z + radius * normal.normal_z / normal_length;
// 平面法向量是给定法线的方向向量 (n_x, n_y, n_z)
float nx = normal.normal_x;
float ny = normal.normal_y;
float nz = normal.normal_z;
// 计算平面方程的 d 值：d = n_x * x1 + n_y * y1 + n_z * z1
float d = nx * x1 + ny * y1 + nz * z1;
// 输出平面方程 n_x * x + n_y * y + n_z * z = d
std::cout << "平面方程: " << nx << " * x + " << ny << " * y + " << nz << " * z = " << d << std::endl;
std::vector<double> res;
res.push_back(nx);
res.push_back(ny);
res.push_back(nz);
res.push_back(d);
return res;
}
/**
* @brief 计算给定点云的法线
* @param 给定点云
*/
pcl::PointCloud<pcl::Normal>::Ptr calNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double radius)
{
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
// 使用 KD-Tree 进行邻域搜索
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
ne.setRadiusSearch(radius);
// 保存计算的法向量
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
ne.compute(*normals);
return normals;
}
/**
* @brief 获取给定坐标的法向量
* @param cloud
* @param normals
* @param p
*/
pcl::Normal getNormalAtPoint(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
pcl::PointCloud<pcl::Normal>::Ptr normals,
const pcl::PointXYZ& p)
{
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(cloud);
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
if (kdtree.nearestKSearch(p, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
int nearest_idx = pointIdxNKNSearch[0];
return normals->points[nearest_idx];
}
else {
throw std::runtime_error("未找到最近邻点，可能点云为空或坐标不在范围内");
}
}
// 将点投影到平面上
std::tuple<pcl::PointXYZ, double> p2Plane(const pcl::PointXYZ& p, std::vector<double> param)
{
double nx = param[0];
double ny = param[1];
double nz = param[2];
double d = param[3];
// 计算法向量的长度
double norm_length = std::sqrt(nx * nx + ny * ny + nz * nz);
// 计算点到平面的距离
double distance = std::abs((p.x * nx + p.y * ny + p.z * nz - d) / norm_length);
// 计算投影点的坐标
double x_proj = p.x - distance * (nx / norm_length);
double y_proj = p.y - distance * (ny / norm_length);
double z_proj = p.z - distance * (nz / norm_length);
pcl::PointXYZ res1(x_proj, y_proj, z_proj);
return std::make_tuple(res1, distance);
}
/**
* @brief 计算给定点云中的点投影在给定平面上的点坐标
* @param cloud 原点云
* @param normal 给定平面的法向量
* @return 投影后的点云
*/
pcl::PointCloud<pcl::PointXYZ>::Ptr projectPointToPlane(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
std::vector<double> param, std::vector<double>& depth)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRes(new pcl::PointCloud<pcl::PointXYZ>);
int count = 0;
for (int i = 0; i < cloud->size(); i++)
{
std::tuple<pcl::PointXYZ, double> res = p2Plane(cloud->points[i], param);
cloudRes->push_back(std::get<0>(res)); // 存储投影点
depth.push_back(std::get<1>(res)); // 存储局部深度
count++;
}
cloudRes->width = count;
cloudRes->height = 1;
return cloudRes;
}
double calculateAngleBetweenNormals(const pcl::Normal& n1, const pcl::Normal& n2) {
// 计算法向量的点积
double dotProduct = n1.normal_x * n2.normal_x + n1.normal_y * n2.normal_y + n1.normal_z * n2.normal_z;
// 确保cosTheta在[-1, 1]范围内
double cosTheta = std::max(-1.0, std::min(1.0, dotProduct));
// 计算夹角
double angle = acos(cosTheta); // 返回值为弧度
// 可选：将角度转换为度
angle = angle * (180.0 / M_PI); // 转换为度
if (angle > 90)
angle = angle - 180;
return angle;
}
// 计算两点之间的距离
double distanceP2P(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)
{
return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
}
// 构造函数
LFSH::LFSH(const pcl::PointXYZ& p, double r, const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int N1, int N2, int N3, double normalR) :
m_P(p), m_R(r), m_SourceCloud(cloud), m_N1(N1), m_N2(N2), m_N3(N3), m_normals(new pcl::PointCloud<pcl::Normal>)
{
std::set<int> idxs = sphereSearch(m_R, m_P, m_SourceCloud);
m_cloud = sphereCloud(idxs, m_SourceCloud); // 球体邻域
pcl::PointCloud<pcl::Normal>::Ptr sourceCloudNormal = calNormals(m_SourceCloud, normalR); // 源点云的法线
for (const auto& point : m_cloud->points)
{ // 从源点云法向量中找出球体邻域内所有点的法向量
try {
pcl::Normal normal = getNormalAtPoint(m_SourceCloud, sourceCloudNormal, point);
m_normals->push_back(normal);
}
catch (const std::runtime_error& e)
{
std::cout << e.what() << std::endl;;
}
}
/* ----------计算局部深度---------- */
pcl::Normal n = getNormalAtPoint(m_cloud, m_normals, m_P); // 待描述点的法线
std::cout << "normal: " << n.normal_x << "\t" << n.normal_y << "\t" << n.normal_z << std::endl;
std::vector<double> planeParam = calculateTangentPlane(m_P, n, m_R); // 计算垂直于待描述点法线，且与球体邻域相切的平面参数
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_proj = projectPointToPlane(m_cloud, planeParam, m_depth); // 计算投影到平面的点坐标，同时计算局部深度
double min_depth = *std::min_element(m_depth.begin(), m_depth.end());
double max_depth = *std::max_element(m_depth.begin(), m_depth.end());
double d_depth = (max_depth - min_depth) / m_N1;
m_depth_histogram.resize(m_N1, 0);
/* ----------计算法线夹角---------- */
for (const auto& it : m_normals->points)
{
m_degress.push_back(calculateAngleBetweenNormals(n, it)); // 存储法线夹角
}
double min_degress = *std::min_element(m_degress.begin(), m_degress.end());
double max_degress = *std::max_element(m_degress.begin(), m_degress.end());
double d_degress = (max_degress - min_degress) / m_N2;
m_degress_histogram.resize(m_N2, 0);
for (size_t i = 0; i < m_depth.size(); i++)
{
// std::cout << "i: " << i << std::endl;
int n1 = (m_depth[i] - min_depth) / d_depth;
if (n1 >= 0 && n1 < m_depth_histogram.size())
{
m_depth_histogram[n1]++;
}
/*std::cout << "n1: " << n1 << std::endl;*/
int n2 = (m_degress[i] - min_degress) / d_degress;
/*std::cout << "m_degress[i]: " << m_degress[i] << std::endl;
std::cout << "n2: " << n2 << std::endl;*/
if (n2 >= 0 && n2 < m_degress_histogram.size())
{
m_degress_histogram[n2]++;
}
}
/* ----------计算密度---------- */
pcl::PointXYZ p_proj = std::get<0>(p2Plane(m_P, planeParam)); // 待描述点的投影
m_denisty_histogram.resize(m_N3, 0);
double max = 0;
for (const auto& it : cloud_proj->points)
{
double distance = distanceP2P(p_proj, it);
max = std::max(max, distance);
}
// bool flag = false;
for (const auto& it : cloud_proj->points)
{
double distance = distanceP2P(p_proj, it);
int num = distance / (max / m_N3);
/*if (flag >= m_denisty_histogram.size())
flag = true;*/
// std::cout << "num: " << num << std::endl;
//std::cout << "dis: " << distance << std::endl;
//std::cout << "num: " << num << std::endl;
if (num >= 0 && num < m_denisty_histogram.size())
{
m_denisty_histogram[num]++;
} // 存储密度
}
/*std::cout << "flag: " << flag << std::endl;
std::cout << "max: " << max << std::endl;*/
}main.cpp
cpp
#define _CRT_SECURE_NO_WARNINGS
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include "LFSH.h"
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("C:\\Users\\10704\\Desktop\\test_data\\0_downsample_1cm.pcd", *cloud);
pcl::PointXYZ p(442429.313, 4130677.500, 14.840);
LFSH lfsh(p, 2, cloud, 10, 15, 5, 1);
for (const auto& it : lfsh.getDepthHistogram())
std::cout << it << "\t";
std::cout << std::endl;
for (const auto& it : lfsh.getDegressHistogram())
std::cout << it << "\t";
std::cout << std::endl;
for (const auto& it : lfsh.getDenistyHistogram())
std::cout << it << "\t";
std::cout << std::endl;
std::cout << "count denisty: " << std::accumulate(lfsh.getDenistyHistogram().begin(), lfsh.getDenistyHistogram().end(), 0) << std::endl;
std::cout << "count degress: " << std::accumulate(lfsh.getDegressHistogram().begin(), lfsh.getDegressHistogram().end(), 0) << std::endl;
std::cout << "count depth: " << std::accumulate(lfsh.getDepthHistogram().begin(), lfsh.getDepthHistogram().end(), 0) << std::endl;
system("pause");
return 0;
}示例
程序运行结果：（某个点的 LFSH）
柱状图：
下一步实验
① 找到直方图相似度的比较方法（即 LFSH 的比较方法，文献中未给出）
② 针对数据测试 LFSH 的匹配效果。