PCL实现点云选取并计算选取点法向量及可视化

1、背景及效果展示

因项目需求，基于PCL1.8.1 + VS2015 实现点云特征点选取并计算选取的特点法向量，并对特征点选取过程可视化、法向量计算结果可视化，特此记录该小功能实现。

2、实现步骤及流程

1）加载/读取源点云数据；

2）执行回调函数、注册屏幕选点事件；

3）记录回调函数中选取的点云、形成点云子集；

4）基于NormalEstimationOMP 加速计算以源点云为搜索表面的点云子集的法向量；

5）调用可视化函数、显示源点云、子集点云、选取特征点、及特征点法向量。

3、代码实现

#include "stdafx.h"
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>//使用OMP需要添加的头文件
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);

using namespace std;
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

// Mutex: //进程锁
boost::mutex cloud_mutex;
PointCloudT::Ptr normalCloud(new PointCloudT);

//用于给回调函数的结构体定义
// structure used to pass arguments to the callback function
struct callback_args
{
	PointCloudT::Ptr clicked_points_3d;
	pcl::visualization::PCLVisualizer::Ptr viewerPtr;
};

//回调函数
void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args)
{
	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);
	
	//TODO
	//data->clicked_points_3d->clear();//将上次选的点清空

	data->clicked_points_3d->points.push_back(current_point);//添加新选择的点
	//pointIndices.push_back(event.getPointIndex());
	normalCloud = data->clicked_points_3d;
	// 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;
	std::cout << event.getPointIndex() << std::endl;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr genCloud(std::vector<float> x, std::vector<float> y, std::vector<float> z)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);

	cloud->width = x.size();
	cloud->height = 1;
	cloud->points.resize(x.size()*1);

	for (int i = 0; i < cloud->width; i++)
	{
		cloud->points[i].x = x[i];
		cloud->points[i].y = y[i];
		cloud->points[i].z = z[i];
	}
	return cloud;
}

void NormalEstimationVisualizer(boost::shared_ptr<pcl::visualization::PCLVisualizer> &_visualizer, 
	pcl::PointCloud<pcl::Normal>::Ptr &_normal,
	PointCloudT::Ptr& _sourceCloud,
	PointCloudT::Ptr& _normalCloud)
{
	// 创建两个观察视点
	int v1(0);
	int v2(1);
	_visualizer->createViewPort(0.0, 0.0, 0.5, 1.0, v1);
	_visualizer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);

	// 设置背景颜色
	_visualizer->setBackgroundColor(0.0, 0.0, 0.0, v1);
	_visualizer->setBackgroundColor(0.0, 0.0, 0.0, v2);

	// 原始的点云设置为白色的
	_visualizer->removePointCloud("cloud_tar_v1");
	_visualizer->removePointCloud("cloud_tar_v2");//删除点云小名，再add达到刷新updatePointCloud效果，同时保留视口信息
	pcl::visualization::PointCloudColorHandlerCustom<PointT> target_cloud_color(_sourceCloud, 20, 180, 20);
	pcl::visualization::PointCloudColorHandlerCustom<PointT> aligned_cloud_color(_normalCloud, 180, 20, 20);
	_visualizer->addPointCloud(_sourceCloud, target_cloud_color, "cloud_tar_v1", v1);//设置原始的点云都是显示为白色
	_visualizer->addPointCloud(_normalCloud, aligned_cloud_color, "clicked_points", v1);//设置原始的点云都是显示为白色
	_visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "clicked_points",v1);
	_visualizer->addPointCloud(_sourceCloud, target_cloud_color, "cloud_tar_v2", v2);
	//添加需要显示的点云法向。cloud为原始点云模型，normal为法向信息，1表示需要显示法向的点云间隔，即每1个点显示一次法向，0.2表示法向长度。
	_visualizer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(_normalCloud, _normal, 1, 0.2,"normal",v2);

	// 加入文本的描述在各自的视口界面
	//在指定视口viewport=v1添加字符串“white 。。。”其中"icp_info_1"是添加字符串的ID标志，（10，15）为坐标16为字符大小 后面分别是RGB值
	_visualizer->addText("Green: Original point cloud\nRed: Selected point cloud", 10, 15, "icp_info_1", v1);
	_visualizer->addText("Green: Original point cloud\nWhite: Calculated Normal", 10, 15, "icp_info_2", v2);

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

int main()
{
	//------------------加载点云数据-------------------
	PointCloudT::Ptr sourceCloud(new PointCloudT);
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Viewer"));
	boost::shared_ptr<pcl::visualization::PCLVisualizer> normalViewer(new pcl::visualization::PCLVisualizer("Normal viewer"));

	if (pcl::io::loadPLYFile<PointT>("C:\\Users\\mateng\\Desktop\\ARglass.ply", *sourceCloud) == -1)
	{
		PCL_ERROR("Could not read file\n");
	}

	//获得互斥体，期间不能修改点云
	cloud_mutex.lock();

	// Display pointcloud:
	//显示点云
	viewer->addPointCloud(sourceCloud);
	//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);
	//Shfit+鼠标左键选择点
	std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
	// Spin until 'Q' is pressed:
	//摁“Q”键结束
	viewer->spin();
	std::cout << "done." << std::endl;
	//释放互斥体
	cloud_mutex.unlock();

	//------------------计算法线----------------------
	pcl::NormalEstimationOMP<PointT, pcl::Normal> n;//OMP加速
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	//建立kdtree来进行近邻点集搜索
	pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>());
	n.setNumberOfThreads(10);//设置openMP的线程数
	//n.setViewPoint(0,0,0);//设置视点，默认为（0，0，0）
	n.setInputCloud(normalCloud);
	n.setSearchSurface(sourceCloud);
	n.setViewPoint(20,20,20);
	n.setSearchMethod(tree);
	n.setKSearch(20);//点云法向计算时，需要所搜的近邻点大小
	//n.setRadiusSearch(0.03);//半径搜素
	n.compute(*normals);//开始进行法向计


    //------------------可视化----------------------
	NormalEstimationVisualizer(normalViewer, normals, sourceCloud, normalCloud);

}

4、总结

本文方法是把选取的特征点作为一个子集并计算该子集在源点云数据下法向量，对于单独特征点云法向量的计算可直接参考pcl中computePointNormal这个函数的使用。感兴趣的朋友可参考链接：PCL只获取点云中一个点的法向量之computePointNormal，文章所述不当之处还请大家指正。

本文参考文章：

PCL学习系列（一）----PCL屏幕选点

PCL 计算点云法向量并显示