PCL库学习笔记（为点云上色的几种方法详解）

近段时间在学习PCL库，在点云上色的问题中进行了一个小总结。分别实现了一下几个功能：

1. 显示点云自带的颜色信息；
2. 根据点云的某个属性进行上色（例如：X,Y,Z等方向上不同颜色）；
3. 自定义单一颜色（给某个点云显示同一个颜色）；
4. 随机上色（由编译器随机给点云分配单一颜色）；
5. 显示点云的法线方向和法向量；

下文会分别介绍上述功能的实现，并提供代码示例：

1、显示点云自带的颜色信息（PointCloudColorHandlerRGBField）

boost::shared_ptr<pcl::visualization::PCLVisualizer> colorHandler(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Cloud"));
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

这里用到了一个类 “PointCloudColorHandlerRGBField” 实际上用这种方法显示点云的自带颜色信息，感觉有点多此一举。可以直接 “viewer->addPointCloud(cloud, “sample cloud”);” 就能实现上述功能。为什么会有这个功能，大家可以尝试用这种方法能否将两个点数相同的不同类型点云，将其中一个点云的颜色赋到另一个点云上进行显示？

2. 根据点云的某个属性进行上色（PointCloudColorHandlerGenericField）；

boost::shared_ptr<pcl::visualization::PCLVisualizer> genericHandler(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Cloud"));
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> rgb(cloud, "y");
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

相信很容易能找到题为“为点云沿着x方向赋予不同颜色”的解决办法，就是利用类“PointCloudColorHandlerGenericField” ，上述代码展示了沿 “y” 方向的渐变上色。那这种方法是否只适用于XYZ方向？其他属性例如曲率，法线等特征是否也可以实现？结果是肯定的，解释如下：
首先我先找到了参数“y”传到了什么地方，通过定位到内部函数如下：

// Get the index we need
    for (size_t d = 0; d < cloud.fields.size (); ++d)
      if (cloud.fields[d].name == field_name)
        return (static_cast<int>(d));
    return (-1);

其中 field_name 就是 “y” 传入的地方，代码实现的功能明显就是从 “cloud.fields” 这个数组中，找到名为 “y” 的编号。那也就是说找到 cloud.fields 就能知道到底还可以传入什么参数？
为了说明这个问题再看一段代码：

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/PCLPointCloud2.h>

int main() {
	pcl::PCLPointCloud2 cloud;
	pcl::PCDReader reader;
	reader.readHeader("C:\\Users\\13427\\Desktop\\test.pcd", cloud);
	for (int i = 0; i < cloud.fields.size(); i++)
	{
		std::cout << cloud.fields[i].name << std::endl;
	}
	return 0;
}

输出为：

x
y
z
normal_x
normal_y
normal_z
rgb
psz

再打开代码中使用到的PCD文件如下：

好的，看到这个图就瞬间明白了，就不多说了。所以按照属性赋值颜色的方法，其关键字可以是点云类型中的任何一个属性。所以，按照曲率，法线，反射强度等特征进行上色工作就能够实现了。

3. 自定义单一颜色（PointCloudColorHandlerCustom）

boost::shared_ptr<pcl::visualization::PCLVisualizer> customHandler(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rgb(cloud, 0, 255, 255);
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

这个类的功能就是为同一个点云中的点赋予相同的颜色，用于区分同一个窗口中的不同点云。值得注意的是，其中 rgb 的取值范围不是 [0, 1] 而是 [0, 255]， 不然出来的点云是黑色的。

4. 随机上色（PointCloudColorHandlerRandom）

boost::shared_ptr<pcl::visualization::PCLVisualizer> randomHandler(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> rgb(cloud);
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

就不再赘述，前面明白了，这都容易了

5. 显示点云的法线方向和法向量（addPointCloudNormals）

boost::shared_ptr<pcl::visualization::PCLVisualizer> normalHandler
						(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr cloud1)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> rgb(cloud, "z");
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, cloud1, 10, 0.05, "normals");
	return viewer;
}

总之，还是要多写，多总结，多深入。虽然写了也可能记不住，但是重复几次说不定就记住了嘞。前车之鉴，复制粘贴一时爽，到头来啥也不会。

最后上一个总的代码：

/* \author Geoffrey Biggs */

#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
// 帮助
void
printUsage(const char* progName)
{
	std::cout << "\n\nUsage: " << progName << " [options]\n\n"
		<< "Options:\n"
		<< "-------------------------------------------\n"
		<< "-s           PointCloudColorHandlerRGBField example\n"
		<< "-g           PointCloudColorHandlerGenericField example\n"
		<< "-c           PointCloudColorHandlerCustom example\n"
		<< "-r           PointCloudColorHandlerRandom example\n"
		<< "-n           Normal visulization example\n"
		<< "\n\n";
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> colorHandler(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Cloud"));
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(cloud);
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> genericHandler(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Cloud"));
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> rgb(cloud, "y");
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> customHandler(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> rgb(cloud, 0, 255, 255);
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> randomHandler(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> rgb(cloud);
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	return viewer;
}

boost::shared_ptr<pcl::visualization::PCLVisualizer> normalHandler
						(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::Normal>::Ptr cloud1)
{
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
	pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> rgb(cloud, "z");
	viewer->addPointCloud(cloud, rgb, "sample cloud");
	viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, cloud1, 10, 0.05, "normals");
	return viewer;
}

// -----Main-----
int
main(int argc, char** argv)
{
	// 解析命令行参数

	printUsage(argv[0]);

	std::cout << "Input your commend: ";
	std::string commend;
	getline(cin, commend);

	bool simple(false), rgb(false), custom_c(false), normals(false),shapes(false);
	if (commend == "-s")
	{
		simple = true;
	}
	else if (commend == "-g")
	{
		rgb = true;
	}
	else if (commend == "-c")
	{
		custom_c = true;
	}
	else if (commend == "-r")
	{
		normals = true;
	}
	else if (commend == "-n")
	{
		shapes = true;
	}

	// 自行创建一随机点云
	pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
	std::cout << "Genarating example point clouds.\n\n";
	// 以椭圆为边线沿z轴拉伸获取其点云，并赋予红绿蓝渐变色。
	uint8_t r(255), g(15), b(15);
	for (float z(-1.0); z <= 1.0; z += 0.05)
	{
		for (float angle(0.0); angle <= 360.0; angle += 5.0)
		{
			pcl::PointXYZ basic_point;
			basic_point.x = 0.5 * cosf(pcl::deg2rad(angle));
			basic_point.y = sinf(pcl::deg2rad(angle));
			basic_point.z = z;
			basic_cloud_ptr->points.push_back(basic_point);

			pcl::PointXYZRGB point;
			point.x = basic_point.x;
			point.y = basic_point.y;
			point.z = basic_point.z;
			uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
				static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
			point.rgb = *reinterpret_cast<float*>(&rgb);
			point_cloud_ptr->points.push_back(point);
		}
		if (z < 0.0)
		{
			r -= 12;
			g += 12;
		}
		else
		{
			g -= 12;
			b += 12;
		}
	}
	basic_cloud_ptr->width = (int)basic_cloud_ptr->points.size();
	basic_cloud_ptr->height = 1;
	point_cloud_ptr->width = (int)point_cloud_ptr->points.size();
	point_cloud_ptr->height = 1;
	// 0.05为搜索半径获取点云法线
	pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
	ne.setInputCloud(point_cloud_ptr);
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>());
	ne.setSearchMethod(tree);
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1(new pcl::PointCloud<pcl::Normal>);
	ne.setRadiusSearch(0.05);
	ne.compute(*cloud_normals1);
	//  0.1为搜索半径获取点云法线
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals2(new pcl::PointCloud<pcl::Normal>);
	ne.setRadiusSearch(0.1);
	ne.compute(*cloud_normals2);

	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
	if (simple)
	{
		viewer = colorHandler(point_cloud_ptr);
	}
	else if (rgb)
	{
		viewer = genericHandler(basic_cloud_ptr);
	}
	else if (custom_c)
	{
		viewer = customHandler(basic_cloud_ptr);
	}
	else if (normals)
	{
		viewer = randomHandler(basic_cloud_ptr);
	}
	else if (shapes)
	{
		viewer = normalHandler(basic_cloud_ptr, cloud_normals1);
	}

	viewer->setBackgroundColor(1, 1, 1);
	viewer->addCoordinateSystem(1.0);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");
	viewer->initCameraParameters();
	
	// 主循环
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
}