无人驾驶传感器融合系列（三）——真实激光雷达点云数据流的处理

无人驾驶传感器融合系列（三）——真实激光雷达点云数据流的处理

本章摘要：在前两章中，讲解了激光雷达点云的分割、聚类基础原理以及实现。这一章主要介绍真实点云情况下的一些预处理，比如点云的导入、过滤、裁剪。然后根据单帧障碍物检测的pipeline，处理点云数据流。

真实激光点云的导入

将file中的点云文件（比如data文件下的 0000000000.pcd ）导入到cloud。

template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr ProcessPointClouds<PointT>::loadPcd(std::string file)
{
    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
    if (pcl::io::loadPCDFile<PointT> (file, *cloud) == -1) //* load the file
    {
        PCL_ERROR ("Couldn't read file \n");
    }
    std::cerr << "Loaded " << cloud->points.size () << " data points from "+file << std::endl;
    return cloud;
}

显示导入的点云

//显示pointXYZ类型的点云
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::string name, Color color)
{
  	viewer->addPointCloud<pcl::PointXYZ> (cloud, name);
  	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
  	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
}
//显示pointXYZI类型的点云
void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, std::string name, Color color)
{
	if(color.r==-1)
	{
		// Select color based off of cloud intensity
		pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> intensity_distribution(cloud,"intensity");
  		viewer->addPointCloud<pcl::PointXYZI>(cloud, intensity_distribution, name);
	}
	else
	{
		// Select color based off input value
		viewer->addPointCloud<pcl::PointXYZI> (cloud, name);
		viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, color.r, color.g, color.b, name);
	}
	viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
}

效果图如下：

过滤导入的点云

从上面的点云可以看出来，点云非常密集，密集的点云会加剧计算量。所以可以适当的对其进行过滤，下面采用VoxelGrid方法对其进行过滤。基本原理就是将点云划分成一个一个的voxel立方格子，每个格子保留一个点，voxel格子边长越大，得到的点云越是稀疏。

typename pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>());
pcl::VoxelGrid<PointT> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (filterRes, filterRes, filterRes);  //filterRes 就为voxel格子的边长。
sor.filter (*cloud_filtered);

裁剪点云

点云过滤之后，会变得适当的稀疏，但是点云的范围依然很广，比如道路外面的树木、建筑物等，这些对我们是没有意义的，还有就是前后特别远的地方，我们也不关心，所以我们需要将这些地方裁剪掉，只保留我们关注的区域。采用CropBox来裁剪点云。

typename pcl::PointCloud<PointT>::Ptr cloud_region (new pcl::PointCloud<PointT>());
pcl::CropBox<PointT> region(true);  //把一定区域内的点云裁剪掉，比如车道外的点，前后太远的点。
region.setMin(minPoint);    //裁剪框x,y,z，i的最小值，比如：Eigen::Vector4f (-30, -6, -2, 0)
region.setMax(maxPoint);    //裁剪框x,y,z, i的最大值。比如：Eigen::Vector4f ( 60, 6, 0, 1)
region.setInputCloud(cloud_filtered);  //输入为上面过滤之后的点云
region.filter(*cloud_region);     //裁剪之后的点云

剔除掉打在车顶上的点云

有些点云会打在车顶上，比如上图中中间那些杂点，这些干扰点没有意义，需要剔除掉。采用CropBox得到一定范围内点的索引，然后采用ExtractIndices剔除掉这些杂点。

std::vector<int> indice;
pcl::CropBox<PointT> roof(true);  
region.setMin(Eigen::Vector4f(-1.5,-1.7,-1.2,1));
region.setMax(Eigen::Vector4f(2.5,1.7,0,1));
region.setInputCloud(cloud_region);
region.filter(indice);      //得到上述范围内点的索引

pcl::PointIndices::Ptr inliers (new pcl::PointIndices());
for(int ind: indice)
   inliers->indices.push_back(ind);

// Create the filtering object
pcl::ExtractIndices<PointT> extract;
// Extract the inliers
extract.setInputCloud (cloud_region);
extract.setIndices (inliers);
extract.setNegative (true);
extract.filter (*cloud_region);

经过上面的过滤、裁剪、剔除之后效果图如下（车顶上的点还没有完全剔除）：

点云数据流的处理

经过上面一系列的预处理之后，就可以按照前两章的内容进行分割，聚类了，至此就完成了单帧点云处理的pipeline。下面说说如何进行点云数据流的处理。

pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI->streamPcd("../src/sensors/data/pcd/data_1");
auto streamIterator = stream.begin();                                         //data_1为存放一系列点云的文件
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;

while (!viewer->wasStopped ())
{
	// Clear viewer
	viewer->removeAllPointClouds();
	viewer->removeAllShapes();

	// Load pcd and run obstacle detection process
    inputCloudI = pointProcessorI->loadPcd((*streamIterator).string());
    cityBlock(viewer, pointProcessorI, inputCloudI);

    streamIterator++;
    if(streamIterator == stream.end())
         streamIterator = stream.begin();

    viewer->spinOnce ();
}

上面用到的streamPcd定义如下：

template<typename PointT>
std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath)
{
    std::vector<boost::filesystem::path> paths(boost::filesystem::directory_iterator{dataPath}, boost::filesystem::directory_iterator{});
    // sort files in accending order so playback is chronological
    sort(paths.begin(), paths.end());

    return paths;
}

最终的效果如下：

完整代码见：github

根据需要，自行调整不同状态时候的输出，代码中已有明确的注释。