PCL热力图显示兔子：按点与点的距离误差、FPFH特征误差显示

按距离显示：第0个点越远越红

第0个点大致在脖子上？

#include 
#include 
#include 
#include 
#include 
#include  
#include 
#include   
#include 
#include 
#include 
using namespace pcl;
using namespace std;
float computeDist(const pcl::PointXYZ& p1, const pcl::PointXYZ& p2)
{
	float dist = 0;
	dist = 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));
	if (isnan(dist))
		return 0;
	return dist;
}
int main()
{
	//按距离映射到颜色上，颜色条生成规则自己定义
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud);
	for (int i = 0; i size(); i++)
	{
		pcl::PointXYZRGB point;
		point.x = cloud->points[i].x;
		point.y = cloud->points[i].y;
		point.z = cloud->points[i].z;
		//计算第0个点到所有点的距离，映射到0到255之间
		float dist = computeDist(cloud->points[0], cloud->points[i])*2500;
		//将距离按规则转换
		uint8_t r, g, b;
		if (dist > 255)
			r = 255;
		else
			r = int(dist);
		b = 255 - r;
		if (r > 125)
			g = b;
		else
			g = r;
		uint32_t rgba = (static_cast(r) << 16 | static_cast(g) << 8 | static_cast(b));
		float rgbf = *reinterpret_cast(&rgba);
		point.rgb = rgbf;
		cloud_rgb->push_back(point);
	}
	//显示
	pcl::visualization::PCLVisualizer viewer("12");
	viewer.setBackgroundColor(0.9, 0.9, 0.9);
	viewer.addPointCloud(cloud_rgb);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return 0;
}
按特征误差显示：也是和第0个点作比较

蓝色区域就是和第0个点的特征比较相似的区域，其他地方误差就有点大了。。。
#include 
#include 
#include 
#include 
#include 
#include 
#include  
#include 
#include 
#include   
#include 
#include 
#include 
#include 
using namespace pcl;
using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud PointCloudT;
float computeDist(pcl::FPFHSignature33& f1, pcl::FPFHSignature33& f2)
{
	float dist_sum = 0;
	for (int i = 0; i < 33; i++)
	{
		dist_sum += (f1.histogram[i] - f2.histogram[i])*(f1.histogram[i] - f2.histogram[i]);
	}
	return dist_sum;
}
void computeNormal(PointCloudT::Ptr cloud, pcl::PointCloud::Ptr cloud_normal)
{
	pcl::NormalEstimation ne;
	ne.setInputCloud(cloud);
	//ne.setRadiusSearch(0.002);
	ne.setKSearch(20);
	ne.compute(*cloud_normal);
}
void computeFPFH(PointCloudT::Ptr cloud, PointCloud::Ptr feats)
{
	PointCloud::Ptr normal(new PointCloud);
	computeNormal(cloud, normal);
	FPFHEstimation fpfh;
	fpfh.setInputCloud(cloud);
	fpfh.setInputNormals(normal);
	//fpfh.setRadiusSearch(0.005);
	fpfh.setKSearch(50);
	fpfh.compute(*feats);
}
int main()
{
	//按距离映射到颜色上，颜色条生成规则自己定义
	pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	PointCloud::Ptr feats(new PointCloud);
	computeFPFH(cloud, feats);
	pcl::PointCloud::Ptr cloud_rgb(new pcl::PointCloud);
	for (int i = 0; i size(); i++)
	{
		pcl::PointXYZRGB point;
		point.x = cloud->points[i].x;
		point.y = cloud->points[i].y;
		point.z = cloud->points[i].z;
		//计算第0个点到所有点的距离，映射到0到255之间
		float dist = computeDist(feats->points[0], feats->points[i]);
		if (i % 100 == 0)
			cout << dist < 255)
			r = 255;
		else
			r = int(dist);
		b = 255 - r;
		if (r > 125)
			g = b;
		else
			g = r;
		uint32_t rgba = (static_cast(r) << 16 | static_cast(g) << 8 | static_cast(b));
		float rgbf = *reinterpret_cast(&rgba);
		point.rgb = rgbf;
		cloud_rgb->push_back(point);
	}
	//显示
	pcl::visualization::PCLVisualizer viewer("12");
	viewer.setBackgroundColor(0.9, 0.9, 0.9);
	viewer.addPointCloud(cloud_rgb);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return 0;
}


作者：zhangruijerry
                    

 
                


                            兔子
                            pcl