快速點特徵直方圖FPFH的可視化

（一）點特徵（PF）

點特徵直方圖是描述通過計算鄰域內所有兩點之間關係而得到的直方圖，其中使用三個角度來描述兩點之間的關係。三個角度的計算如下圖所示。因此，在計算PFH時需要計算每個點的法向量，依據相鄰點法向量計算得到。

在推導計算三個角度時，感覺應該是均要加arccos，另外在推導第3個角度時，不清楚是怎麼推理的，有知道的留下言。

（二）點特徵直方圖（PFH）

將每個特徵值（只考慮三個角度）範圍劃分成b個子區間，將每兩個點計算得到的角度劃入到相應的子區間內，並統計落在每個子區間的點數目，依據統計的數目即可得到得到直方圖。（說一點，有人提問x、y軸表示什麼，很明顯，x軸爲劃分的子區間，其將3個子區間進行了累加，所以長度爲b*3，y軸爲落入該子區間內點的數目。）假設某激光點的鄰域內有n個點，計算鄰域內所有兩點間的三參數，可得到個三元素，把三個角度分別劃分成b個子區間，統計鄰域內個元素落在子區間的數目，即可得到3*b個子區間的直方圖。

（三）快速點特徵直方圖（FPFH）

已知點雲P中有n個點，那麼它的點特徵直方圖（PFH）的理論計算複雜度是O(nk^2)，其中k是點雲P中每個點p計算特徵向量時考慮的鄰域數量。對於實時應用或接近實時應用中，密集點雲的點特徵直方圖（PFH）的計算，是一個主要的性能瓶頸。PFH計算方式的簡化形式，稱爲快速點特徵直方圖FPFH（Fast Point Feature Histograms），FPFH把算法的計算複雜度降低到了，但是仍然保留了PFH大部分的識別特性。
計算步驟如下：

第一步我們先計算了每個查詢點Pq的一系列值，並把它叫做SPFH(Simplified Point Feature Histgram)

第二步重新計算每個點的k近鄰，使用鄰近的SPFH值計算Pq的最終直方圖，如下所示：

權重Wk在一些給定的度量空間中，表示查詢點Pq和其近鄰點Pk之間的距離，可以用來評定一對點（Pq，Pk）。Wk的定義，可根據需要自己設置。

對於一個已知查詢點Pq，首先利用Pq和它鄰域點之間對應對（下圖中紅色線）來估算它的SPFH，很顯然其比PFH的標準計算少了鄰域點之間的互聯。其次，點雲數據集中所有點都要執行這一計算獲取SPFH，接下來使用它的鄰近點Pk的SPFH值和Pq點的SPFH值重新計算，從而得到最終Pq點的最終FPFH值。FPFH計算添加的計算連接對，在如下圖中使用黑色線表示，一些重要點對（與Pq直接相連的點）被重複計算兩次（圖中以粗線來表示），而其他間接相連的用細黑線表示。

PFH與FPFH之間的差異

1.FPFH沒有和它所有的近鄰有着聯繫，因此可能會丟失一些值的匹配。

2.PFH模型可以更精確的描述表面，而FPFH則包括了額外的點的配對在半徑爲r的圓的外面(最多不會超過2r)

3.因爲權重的組合，FPFH結合了SPFH的值並且重新獲取了一些點的近鄰。

4.FPFH複雜度大大降低，計算更快。

5.最終的直方圖是簡化了。

（四）用鼠標點擊點雲中任意點顯示該點的FPFH直方圖

PCL中提供計算每個點的FPFH，下面代碼直接實現點擊特定點，直方圖上顯示相應的FPFH。按住shift+鼠標左鍵，實現點選

#include <pcl/point_cloud.h>
#include <iostream>
#include <pcl/io/pcd_io.h> 
#include <pcl/point_types.h> 
#include <pcl/visualization/cloud_viewer.h> 
#include <pcl/visualization/histogram_visualizer.h>
#include <pcl/point_types.h>
#include <pcl/features/fpfh.h>
#include <pcl/filters/fast_bilateral.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/features/normal_3d_omp.h>
#include"IO.h"

typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

// Mutex: //
boost::mutex cloud_mutex;
pcl::visualization::PCLPlotter plotter;
pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(new pcl::PointCloud<pcl::FPFHSignature33>());

struct callback_args {
	// structure used to pass arguments to the callback function
	PointCloudT::Ptr clicked_points_3d;
	pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};

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;
	PointT 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<PointT> 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.setXTitle("Range");
	plotter.setYTitle("Number");
	plotter.plot();
}

void main()
{
	IO IOExample;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
	cloud_src = IOExample.XYZ2PCDPtr("D:\\FPFH.txt");

	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud_src);
	ne.setSearchMethod(tree);
	ne.setRadiusSearch(1.5);
	ne.compute(*normals);


	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_fpfh(new pcl::search::KdTree<pcl::PointXYZ>);

	fpfh.setSearchSurface(cloud_src);//輸入完整點雲數據
	fpfh.setInputCloud(cloud_src); // 輸入關鍵點
	fpfh.setInputNormals(normals);
	fpfh.setRadiusSearch(2.0);
	fpfh.setSearchMethod(tree);
	fpfh.compute(*fpfhs);


	// Display pointcloud:
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer"));

	//pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_src, "z");//按照z字段進行渲染
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> fildColor(cloud_src, 255, 255, 255);//將原始點雲設置成綠色對象 single_color
	viewer->addPointCloud(cloud_src, fildColor, "sample");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample");
	viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);

	// Add point picking callback to viewer:
	struct callback_args cb_args;
	PointCloudT::Ptr clicked_points_3d(new PointCloudT);
	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;

	// Spin until 'Q' is pressed:
	viewer->spin();
	std::cout << "done." << std::endl;

	cloud_mutex.unlock();

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}

機載點雲中，幾類有代表性的FPFH：（默認將特徵空間分成11等分，只統計3個角度，因此X軸維度爲33，y軸爲統計落入該子區間內點數量）

建築物

平坦地面：

植被：

若干個點同時顯示：

#include<pcl/visualization/pcl_plotter.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/console/parse.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/normal_space.h>
#include <pcl/common/eigen.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/visualization/histogram_visualizer.h>
#include<iostream>

using namespace std;
using namespace pcl::visualization;
using namespace pcl::console;
#include"IO.h"
int
main()
{
	IO IOExample;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_src(new pcl::PointCloud<pcl::PointXYZ>);
	cloud_src = IOExample.XYZ2PCDPtr("D:\\smallData.txt");

	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud_src);
	ne.setSearchMethod(tree);
	ne.setRadiusSearch(1.0);
	ne.compute(*normals);


	pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh;
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_fpfh(new pcl::search::KdTree<pcl::PointXYZ>);

	fpfh.setSearchSurface(cloud_src);//輸入完整點雲數據
	fpfh.setInputCloud(cloud_src); // 輸入關鍵點
	fpfh.setInputNormals(normals);
	fpfh.setRadiusSearch(1.2);
	fpfh.setSearchMethod(tree);
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfh_src(new pcl::PointCloud<pcl::FPFHSignature33>);
	fpfh.compute(*fpfh_src);


	//定義繪圖器
	PCLPlotter *plotter = new PCLPlotter("FPFH");
	plotter->setXTitle("Range");
	plotter->setYTitle("Number");
	//設置特性
	plotter->setShowLegend(true);
	std::cout << pcl::getFieldsList<pcl::FPFHSignature33>(*fpfh_src);

	//顯示
	int indx;
	for (int m = 0; m < 6; m++)
	{
		plotter->addFeatureHistogram<pcl::FPFHSignature33>(*fpfh_src, "fpfh", m, std::to_string(m)/*"one_fpfh"*/);
		plotter->setWindowSize(800, 600);
		plotter->spinOnce(100);
	}
	
	//計算FPFH的點
	pcl::PointCloud<pcl::PointXYZ>::Ptr selected_FPFH(new pcl::PointCloud<pcl::PointXYZ>);
	selected_FPFH->width = 6;
	selected_FPFH->height = 1;
	selected_FPFH->is_dense = false;
	selected_FPFH->resize(selected_FPFH->width*selected_FPFH->height);
	selected_FPFH->points[0] = cloud_src->points[0];
	selected_FPFH->points[1] = cloud_src->points[100];
	selected_FPFH->points[2] = cloud_src->points[300];
	selected_FPFH->points[3] = cloud_src->points[500];
	selected_FPFH->points[4] = cloud_src->points[700];
	selected_FPFH->points[5] = cloud_src->points[1000];

	//原始點雲數據
	pcl::visualization::PCLVisualizer viewer("Select Point for FPFH");
	viewer.setBackgroundColor(0, 0, 0);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_src, 255,255, 255);
	viewer.addPointCloud<pcl::PointXYZ>(cloud_src, single_color, "sample");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample");

	//選中的點雲
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> FPFH_color(selected_FPFH, 255, 0, 0);
	viewer.addPointCloud<pcl::PointXYZ>(selected_FPFH, FPFH_color, "sample01");
	viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample01");
	while (!viewer.wasStopped())
	{
		viewer.spinOnce(1);
	}
	
	return 1;
}

參考博文及文獻：

[1]https://blog.csdn.net/qq_25491201/article/details/51105826

[2]https://blog.csdn.net/datase/article/details/84960635

[3]基於多尺度自適應特徵的機載LiDAR點雲分類

快速點特徵直方圖FPFH的可視化

常用的 Git 指令

sm4加密工具類

MATLAB中表示點形狀、顏色的常見符號

快速點特徵直方圖FPFH的可視化

各階範數的計算

幾種基於直方圖的圖像全局二值化算法

彙總條件隨機場知識（一）

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結