先来看 pcl::FPFHSignature33 的源码:
/** \brief A point structure representing the Fast Point Feature Histogram (FPFH). * \ingroup common */ struct FPFHSignature33 { float histogram[33]; static int descriptorSize () { return 33; }
friend std::ostream& operator << (std::ostream& os, const FPFHSignature33& p);
};
这里就是一个很简单 struct,重要的数据成员就是一个浮点型数组,存储的是点云中某点的FPFH特征,一共33个值。
那么什么是FPFH特征呢?
点快速特征直方图(Fast Point Feature Histogram, FPFH)通俗地来说就是表示三维点的一种特征,类似二维图像中的SIFT、SURF、ORB特征等,都是携带了某种特定的信息。类似二维图像的配准,FPFH也可以用于三维点云之间的配准。
FPFH在PCL中的实现是pcl_features库的一部分。默认的FPFH实现使用11个统计子区间,特征直方图被分别计算然后合并得出了浮点值的一个33元素的特征向量,这些值就保存在pcl::FPFHSignature33中。
那么FPFH的求解过程又是怎样呢?这里我们需要先来了解以下它的前身——PFH。
点特征直方图(PFH)是一种的姿态不变的局部特征。其计算方式是通过参数化查询点与邻域点之间的空间差异,并形成一个多维直方图对点的k邻域内的几何属性进行描述。直方图所在的高维超空间为特征表示提供了一个可度量的信息,对点云对应曲面的6维姿态来说它具有不变性,并且在不同的采样密度或邻域的噪音等级下具有鲁棒性。
点特征直方图(PFH)表示法是基于点与其k邻域之间的关系以及它们的估计法线。它考虑估计法线方向之间所有的相互作用,试图捕获最好的样本表面变化情况,以描述样本的几何特征。因此,合成的特征超空间取决于每个点的表面法线估计的质量。简而言之,某点的PFH跟其邻域内每个近邻点以及每个点的法线有关。
图1表示的是一个查询点(pq) 的PFH计算的影响区域。pq 用红色标注并放在圆球的中间位置,邻域半径为r, pq的所有k近邻元素(即与点pq的距离小于半径r的所有点)全部互相连接在一个网络中。最终的PFH描述子需要通过计算邻域内所有两点之间关系而得到,因此其计算复杂度是O(k2) 。
KaTeX parse error: Expected '}', got 'EOF' at end of input: …j−pi)×uw=u×v
这里的 ( α , ϕ , θ ) ( α , ϕ , θ ) ( α , ϕ , θ ) (α,ϕ,θ)(\alpha, \phi, \theta )(α,ϕ,θ) (α,ϕ,θ)(α,ϕ,θ)(α,ϕ,θ)就是某点的PFH特征的三个元素,见图2:
在PCL的源码中用以下函数来计算点对间的三个角度特征元素:
/** \brief 计算点对的PFH特征的4个特征元素值,包含3个角度值和一个两点间的距离值
* \param[in] cloud 点云集
* \param[in] normals 法线集
* \param[in] p_idx 第一个点(source)在点云中的索引
* \param[in] q_idx 第二个点(target)在点云中的索引
* \param[out] f1 第一个角度特征值 (angle between the projection of nq_idx and u)
* \param[out] f2 第二个角度特征值 (angle between nq_idx and v)
* \param[out] f3 第三个角度特征值 (angle between np_idx and |p_idx - q_idx|)
* \param[out] f4 两点间的欧式距离
*/
bool computePairFeatures (const pcl::PointCloud &cloud,
const pcl::PointCloud &normals,
int p_idx, int q_idx,
float &f1, float &f2, float &f3, float &f4);
这里的f1、f2、f3就是点对间PFH特征的三个元素。计算出这三个元素值后,PCL中通过下面这个函数获取该查询点的PFH:
/** \brief 由点的近邻及各自的法线计算其PFH
* \param[in] cloud 点云集
* \param[in] normals 法线集
* \param[in] indices 点云集中的k个近邻的索引
* \param[in] nr_split 每个角度特征需要划分的份数(默认为5)
* \param[out] pfh_histogram 查询点最终的PFH
*/
void computePointPFHSignature (const pcl::PointCloud &cloud,
const pcl::PointCloud &normals,
const std::vector &indices, int nr_split,
Eigen::VectorXf &pfh_histogram);
快速点特征直方图(Fast Point Feature Histograms, FPFH)是PFH计算方式的简化形式。它的思想在于分别计算查询点的k邻域中每一个点的简化点特征直方图(Simplified Point Feature Histogram,SPFH),再通过一个公式将所有的SPFH加权成最后的快速点特征直方图。FPFH把算法的计算复杂度降低到了O(nk) ,但是任然保留了PFH大部分的识别特性。
其中权重wk表示查询点p与给定度量空间中的近邻点pk之间的距离。
对于计算速度要求苛刻的用户,PCL提供了一个FPFH估计的另一实现,它使用多核/多线程规范,利用OpenMP开发模式来提高计算速度。这个类的名称是pcl::FPFHEstimationOMP,并且它的应用程序接口(API)100%兼容单线程pcl::FPFHEstimation,这使它适合作为一个替换元件。在8核系统中,OpenMP的实现可以在6-8倍更快的计算时间内完全同样单核系统上的计算。
/** \brief 根据(f1, f2, f3)三个特征值计算单个SPFH
* \param[in] cloud 输入点云
* \param[in] normals 法线信息
* \param[in] p_idx 查询点(source)
* \param[in] row 在特征直方图里对应的行号
* \param[in] indices k近邻索引
* \param[out] hist_f1 f1特征矩阵
* \param[out] hist_f2 f2特征矩阵
* \param[out] hist_f3 f3特征矩阵
*/
template void
pcl::FPFHEstimation::computePointSPFHSignature (
const pcl::PointCloud &cloud, const pcl::PointCloud &normals,
int p_idx, int row, const std::vector &indices,
Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
{
Eigen::Vector4f pfh_tuple;
// 从直方图矩阵中获取特征的分隔数bins
int nr_bins_f1 = static_cast (hist_f1.cols ());
int nr_bins_f2 = static_cast (hist_f2.cols ());
int nr_bins_f3 = static_cast (hist_f3.cols ());
// Factorization constant
float hist_incr = 100.0f / static_cast
// 对近邻点进行迭代计算
for (size_t idx = 0; idx < indices.size (); ++idx)
{
// 对自身不计算
if (p_idx == indices[idx])
continue;
// 对查询点p_idx以及它的某一近邻这对点计算三个特征值
if (!computePairFeatures (cloud, normals, p_idx, indices[idx], pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3]))
continue;
// 归一化f1, f2, f3 并存入直方图中
int h_index = static_cast<int> (floor (nr_bins_f1 * ((pfh_tuple[0] + M_PI) * d_pi_)));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f1) h_index = nr_bins_f1 - 1;
hist_f1 (row, h_index) += hist_incr; // 对应的区间中落了值,统计的直方图数量+100/k
h_index = static_cast<int> (floor (nr_bins_f2 * ((pfh_tuple[1] + 1.0) * 0.5)));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f2) h_index = nr_bins_f2 - 1;
hist_f2 (row, h_index) += hist_incr;
h_index = static_cast<int> (floor (nr_bins_f3 * ((pfh_tuple[2] + 1.0) * 0.5)));
if (h_index < 0) h_index = 0;
if (h_index >= nr_bins_f3) h_index = nr_bins_f3 - 1;
hist_f3 (row, h_index) += hist_incr;
}
}
/** \brief 加权SPFH以计算最终的FPFH
// 获取特征分隔数bins
int nr_bins_f1 = static_cast
int nr_bins_f2 = static_cast
int nr_bins_f3 = static_cast
int nr_bins_f12 = nr_bins_f1 + nr_bins_f2;
// 清空直方图,这里共33个
fpfh_histogram.setZero (nr_bins_f1 + nr_bins_f2 + nr_bins_f3);
// Use the entire patch
for (size_t idx = 0, data_size = indices.size (); idx < data_size; ++idx)
{
// 查询点本身不做计算
if (dists[idx] == 0)
continue;
// 标准权值为查询点到此近邻的距离的倒数
weight = 1.0f / dists[idx];
// 对查询点及其近邻的SPFH进行加权
for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
{
val_f1 = hist_f1 (indices[idx], f1_i) * weight;
sum_f1 += val_f1;
fpfh_histogram[f1_i] += val_f1;
}
for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
{
val_f2 = hist_f2 (indices[idx], f2_i) * weight;
sum_f2 += val_f2;
fpfh_histogram[f2_i + nr_bins_f1] += val_f2;
}
for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
{
val_f3 = hist_f3 (indices[idx], f3_i) * weight;
sum_f3 += val_f3;
fpfh_histogram[f3_i + nr_bins_f12] += val_f3;
}
}
if (sum_f1 != 0)
sum_f1 = 100.0 / sum_f1; // histogram values sum up to 100
if (sum_f2 != 0)
sum_f2 = 100.0 / sum_f2; // histogram values sum up to 100
if (sum_f3 != 0)
sum_f3 = 100.0 / sum_f3; // histogram values sum up to 100
// 调整最终的FPFH值,乘了100
for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
fpfh_histogram[f1_i] *= static_cast
for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
fpfh_histogram[f2_i + nr_bins_f1] *= static_cast
for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
fpfh_histogram[f3_i + nr_bins_f12] *= static_cast
}
/** \brief 计算FPFH描述子,最终公式 */
template
pcl::FPFHEstimation
{
std::vector
std::vector
std::vector
// 计算查询点的SPFH特征,得到SPFH特征查询表
computeSPFHSignatures (spfh_hist_lookup, hist_f1_, hist_f2_, hist_f3_);
output.is_dense = true;
// 如果输入点云是dense的,就不用对每个点坐标的有效性进行判断(针对无效值NaN/和无限值Inf)
if (input_->is_dense)
{
// 对每个近邻进行迭代计算
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
// 找到每个近邻的k近邻
if (this->searchForNeighbors((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
// 如果某一近邻的近邻查找失败,则输出点云非dense,FPFH置为NaN
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = std::numeric_limits
output.is_dense = false;
continue;
}
// 某一近邻的近邻查找成功, 将近近邻索引值映射到SPFH矩阵里的行号
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// 加权计算FPFH特征
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// 将FPFH特征输出
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
}
}
else // 输入点云非dense,加了一个坐标值有效性判断,其他一样
{
// Iterate over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = std::numeric_limits
output.is_dense = false;
continue;
}
// ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices
// instead of indices into surface_->points
for (size_t i = 0; i < nn_indices.size (); ++i)
nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
// Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
// ...and copy it into the output cloud
for (int d = 0; d < fpfh_histogram_.size (); ++d)
output.points[idx].histogram[d] = fpfh_histogram_[d];
}
}
}
实验设备:惠普ProDesk 680 G1 TWR
配置: 处理器Intel Corei5 4590,内存4G
操作系统:Ubuntu 14.04
实验源码:
#include
#include
#include
#include // 法线
#include
#include // 可视化
#include
#include
#include
using namespace std;
boost::mutex cloud_mutex;
pcl::visualization::PCLPlotter plotter;
//pcl::FPFHEstimation
pcl::FPFHEstimationOMP
pcl::FPFHEstimation
pcl::PointCloud
// structure used to pass arguments to the callback function
struct callback_args {
pcl::PointCloud
pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};
// callback function
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
plotter.clearPlots();
struct callback_args* data = (struct callback_args *)args;
if (event.getPointIndex() == -1)
return;
pcl::PointXYZ current_point;
event.getPoint(current_point.x, current_point.y, current_point.z);
data->clicked_points_3d->points.clear();
data->clicked_points_3d->points.push_back(current_point);
// Draw clicked points in red:
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red(data->clicked_points_3d, 255, 0, 0);
data->viewerPtr->removePointCloud("clicked_points");
data->viewerPtr->addPointCloud(data->clicked_points_3d, red, "clicked_points");
data->viewerPtr->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10, "clicked_points");
std::cout << current_point.x << " " << current_point.y << " " << current_point.z << std::endl;
int num = event.getPointIndex();
plotter.addFeatureHistogram<pcl::FPFHSignature33>(*fpfhs, "fpfh", num);
plotter.plot();
}
int main(int argc, char *argv[]) {
if (argc < 2) {
printf(" – Usage: %s
return -1;
}
bool display = true;
bool downSampling = false;
// load pcd/ply point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr model(new pcl::PointCloud<pcl::PointXYZ>()); // 模型点云
if (pcl::io::loadPCDFile(argv[1], *model) < 0) {
std::cerr << "Error loading model cloud." << std::endl;
return -1;
}
std::cout << "Cloud size: " << model->points.size() << std::endl;
// cloud_mutex.lock();// for not overwriting the point cloud
if (downSampling) {
// create the filtering object
std::cout << "Number of points before downSampling: " << model->points.size() << std::endl;
pcl::VoxelGrid
sor.setInputCloud(model);
sor.setLeafSize(0.01, 0.01, 0.01);
sor.filter(*model);
std::cout << "Number of points after downSampling: " << model->points.size() << std::endl;
}
// Normal estimation
auto t1 = chrono::steady_clock::now();
pcl::NormalEstimation<pcl::PointXYZ, pcl::PointNormal> ne;
pcl::PointCloud<pcl::PointNormal>::Ptr normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setInputCloud(model);
ne.setSearchMethod(tree);
ne.setKSearch(10);
// ne.setRadiusSearch(0.03);
ne.compute(*normals);
auto t2 = chrono::steady_clock::now();
auto dt = chrono::duration_cast
cout << "Time cost of Normal estimation: " << dt << endl;
// fpfh or fpfh_omp
fpfh_omp.setInputCloud(model);
fpfh_omp.setInputNormals(normals);
fpfh_omp.setSearchMethod(tree);
fpfh_omp.setNumberOfThreads(8);
fpfh_omp.setRadiusSearch(0.05);
fpfh_omp.compute(*fpfhs);
// fpfh.setInputCloud(model);
// fpfh.setInputNormals(normals);
// fpfh.setSearchMethod(tree);
// fpfh.setRadiusSearch(0.05);
// fpfh.compute(*fpfhs);
t1 = chrono::steady_clock::now();
dt = chrono::duration_cast
cout << "Time cost of FPFH estimation: " << dt << endl;
pcl::FPFHSignature33 descriptor;
for (int i=0; i<10; ++i) {
int index = i + rand() % model->points.size();
descriptor = fpfhs->points[index];
std::cout << " -- fpfh for point "<< index << ":\n" << descriptor << std::endl;
}
if (display) {
plotter.addFeatureHistogram<pcl::FPFHSignature33>(*fpfhs, "fpfh",100);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(model, "model");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "model");
viewer->addPointCloudNormals<pcl::PointXYZ, pcl::PointNormal>(model, normals, 10, 0.05, "normals"); // display every 1 points, and the scale of the arrow is 10
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
// Add point picking callback to viewer:
struct callback_args cb_args;
pcl::PointCloud<pcl::PointXYZ>::Ptr clicked_points_3d(new pcl::PointCloud<pcl::PointXYZ>);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);
std::cout << "Shift + click on three floor points, then press 'Q'..." << std::endl;
// viewer->spin();
// cloud_mutex.unlock();
while (!viewer->wasStopped()) {
viewer->spinOnce(100); // Spin until 'Q' is pressed
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
return 0;
}
实验一用传统的pcl::FPFHEstimation实现,123870个点计算FPFH耗时0.5s左右。
实验二用带加速的pcl::FPFHEstimationOMP实现,123870个点计算FPFH耗时0.2s左右。
红点表示选择的查询点,直方图为查询点的FPFH。
控制台输出了随机10个点的FPFH特征,每个FPFH特征有33个数据。
Fast Point Fcaturc Histograms (FPFH) for 3D Registration ICRA2009 - 论文
Fast Point Feature Histograms (FPFH) descriptors - PCL官网教程
PCL/OpenNI tutorial 4: 3D object recognition (descriptors)
pcl里面的点特征直方图(PFH) - CSDN
PCL库之快速点特征直方图(FPFH)描述子 - CSDN
快速点特征直方图(FPFH)描述子 - PCL中国
三维配准中FPFH特征提取算法 - 北京化工大学研究毕业设计(论文)