無人駕駛傳感器融合系列（三）——真實雷射雷達點雲資料流的處理

本章摘要：在前兩章中，講解了雷射雷達點雲的分割、聚類基礎原理以及實作。這一章主要介紹真實點雲情況下的一些預處理，比如點雲的導入、過濾、裁剪。然後根據單幀障礙物檢測的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

根據需要，自行調整不同狀态時候的輸出，代碼中已有明确的注釋。