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

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

第0个点大致在脖子上？

#include <iostream>
#include <vector>
#include <algorithm>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h> 
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>  
#include <Eigen/Dense>
#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>


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<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
	for (int i = 0; i < cloud->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<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
		float rgbf = *reinterpret_cast<float*>(&rgba);
		point.rgb = rgbf;
		cloud_rgb->push_back(point);
	}

	//显示
	pcl::visualization::PCLVisualizer viewer("12");
	viewer.setBackgroundColor(0.9, 0.9, 0.9);
	viewer.addPointCloud<pcl::PointXYZRGB>(cloud_rgb);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return 0;
}

按特征误差显示：也是和第0个点作比较

蓝色区域就是和第0个点的特征比较相似的区域，其他地方误差就有点大了。。。

#include <string>
#include <iostream>

#include <vector>
#include <algorithm>

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h> 
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>  
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh.h>
#include <pcl/search/kdtree.h>
#include <pcl/kdtree/kdtree_flann.h>


using namespace pcl;
using namespace std;

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> 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<pcl::Normal>::Ptr cloud_normal)
{
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setInputCloud(cloud);
	//ne.setRadiusSearch(0.002);
	ne.setKSearch(20);
	ne.compute(*cloud_normal);
}

void computeFPFH(PointCloudT::Ptr cloud, PointCloud<FPFHSignature33>::Ptr feats)
{
	PointCloud<pcl::Normal>::Ptr normal(new PointCloud<pcl::Normal>);
	computeNormal(cloud, normal);
	FPFHEstimation<PointXYZ, Normal, FPFHSignature33> fpfh;
	fpfh.setInputCloud(cloud);
	fpfh.setInputNormals(normal);
	//fpfh.setRadiusSearch(0.005);
	fpfh.setKSearch(50);
	fpfh.compute(*feats);
}

int main()
{
	//按距离映射到颜色上，颜色条生成规则自己定义
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<PointXYZ>);
	pcl::io::loadPCDFile("bunny.pcd", *cloud);
	PointCloud<FPFHSignature33>::Ptr feats(new PointCloud<FPFHSignature33>);
	computeFPFH(cloud, feats);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb(new pcl::PointCloud<pcl::PointXYZRGB>);
	for (int i = 0; i < cloud->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 << endl;
		//将距离按规则转换
		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<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
		float rgbf = *reinterpret_cast<float*>(&rgba);
		point.rgb = rgbf;
		cloud_rgb->push_back(point);
	}

	//显示
	pcl::visualization::PCLVisualizer viewer("12");
	viewer.setBackgroundColor(0.9, 0.9, 0.9);
	viewer.addPointCloud<pcl::PointXYZRGB>(cloud_rgb);
	while (!viewer.wasStopped())
	{
		viewer.spinOnce();
	}
	return 0;
}