Udacity传感器融合笔记（二）lidar 点云处理

本篇博客主要记录lidar点云的分割以及聚类。

1.Segmentation 点云分割
对于lidar反馈回来的数据，我们总是希望从中准确的找到那些是障碍物，那些是道路点。为此，udacity课程中使用了RANSAC算法来进行点云的分割，将lidar点云分为障碍物点（外点）和道路点（内点）两类。
1.1 RANSAC
对于RANSAC算法（随即抽样一致算法），初次接触是在高博的视觉SLAM14讲中，该算法可以从一组包含“局外点”的数据中通过迭代的方式估计数学模型的参数，从而找到符合模型的“局内点”。这里不多做介绍，详细原理可以推荐一篇博客
RANSAC深度解析
在udacity的课程中，也对RANSAC的原理进行了简要介绍和仿真。RANSAC是一种检测数据中异常值的方法。 RANSAC运行最大迭代次数，并返回最合适的模型。 每次迭代都会随机选择数据的子样本，并通过它拟合模型，例如直线或平面。 然后，将具有最大数量的内部值或最低噪声的迭代用作最佳模型。一种类型的RANSAC选择适合的最小点子集。 对于一条线，这将是两个点，对于平面，则将是三个点。 然后，通过迭代遍历每个剩余点并计算其与模型的距离，来计算内部数。 距模型一定距离内的点计为内部值。 那么，具有最大内部数的迭代就是最佳模型。 这也是udacity课程中实现的版本。RANSAC的其他方法可以对模型点的某些百分比进行采样，例如占总点数的20％，然后对其拟合一条线。 然后计算该行的误差，误差最小的迭代就是最佳模型。 由于不需要考虑每次迭代的每个点，因此该方法可能具有一些优点。
1.2 RANSAC仿真
接下来是ransac的仿真部分代码，这里只记录平面点云的RANSAC分割

main函数

int main ()
{
    // Create viewer 创建可视化窗口
    pcl::visualization::PCLVisualizer::Ptr viewer = initScene();
    // Create data  创建点云数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = CreateData3D();
    // TODO: Change the max iteration and distance tolerance arguments for Ransac function 设定最大迭代次数和距离阈值
    std::unordered_set<int> inliers = Ransac(cloud, 100, 0.5);
    pcl::PointCloud<pcl::PointXYZ>::Ptr  cloudInliers(new       pcl::PointCloud<pcl::PointXYZ>());
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudOutliers(new pcl::PointCloud<pcl::PointXYZ>());
    
    for(int index = 0; index < cloud->points.size(); index++)
    {
        //对点云进行分割
        pcl::PointXYZ point = cloud->points[index];
        if(inliers.count(index))
            cloudInliers->points.push_back(point);
        else
            cloudOutliers->points.push_back(point);
    }
    // Render 2D point cloud with inliers and outliers，对分割后的点云进行渲染 内点绿色，外点红色
    if(inliers.size())
    {
        renderPointCloud(viewer,cloudInliers,"inliers",Color(0,1,0));
        renderPointCloud(viewer,cloudOutliers,"outliers",Color(1,0,0));
    }
    else
    {
        //其他背景黑色
        renderPointCloud(viewer,cloud,"data");
    }

    while (!viewer->wasStopped ())
    {
        viewer->spinOnce ();
    }

}

加载pcd函数

//加载pcd文件
pcl::PointCloud<pcl::PointXYZ>::Ptr CreateData3D()
{
    ProcessPointClouds<pcl::PointXYZ> pointProcessor; //创建pointProcessor
    return pointProcessor.loadPcd("../../../sensors/data/pcd/simpleHighway.pcd"); //加载data中的pcd文件
}

创建可视化窗口并初始化

//创建窗口并初始化函数
pcl::visualization::PCLVisualizer::Ptr initScene()
{
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer ("2D Viewer"));//创建可视化窗口
    viewer->setBackgroundColor (0, 0, 0); //背景颜色黑色
    viewer->initCameraParameters(); //初始化窗口参数
    viewer->setCameraPosition(0, 0, 15, 0, 1, 0);//窗口位置及视角
    viewer->addCoordinateSystem (1.0);//创建坐标系
    return viewer;
}

RANSAC算法

std::unordered_set<int> Ransac(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int maxIterations, float distanceTol)
{
    std::unordered_set<int> inliersResult; //创建关联容器 用于储存内点  std::unordered_set是SLT中的一种容器，基于RB-Tree结构，有机会后面介绍
    srand(time(NULL));
    auto startTime = std::chrono::steady_clock::now();

    // TODO: Fill in this function
    //算法步骤
    // For max iterations
    // Randomly sample subset and fit line
    // Measure distance between every point and fitted line
    // If distance is smaller than threshold count it as inlier
    // Return indicies of inliers from fitted line with most inliers
    while(maxIterations--)
    {
        std::unordered_set<int> inliers; //内点容器
        while(inliers.size()<3)
            inliers.insert(rand()%(cloud->points.size())); //随机选3个点组成平面插入进inlier中

        float x1,x2,x3,y1,y2,y3,z1,z2,z3;

        auto itr = inliers.begin();//将inlier的开始地址赋给itr
        x1 = cloud->points[*itr].x;
        y1 = cloud->points[*itr].y;
        z1 = cloud->points[*itr].z;
        itr++;
        x2 = cloud->points[*itr].x;
        y2 = cloud->points[*itr].y;
        z2 = cloud->points[*itr].z;
        itr++;
        x3 = cloud->points[*itr].x;
        y3 = cloud->points[*itr].y;
        z3 = cloud->points[*itr].z;

        //三点平面公式计算 a b c d
        float a = (y2-y1)*(z3-z1)-(z2-z1)*(y3-y1);
        float b = (z2-z1)*(x3-x1)-(x2-x1)*(z3-z1);
        float c = (x2- x1)*(y3-y1)-(y2-y1)*(x3-x1);
        float d = -(a*x1+b*y1+c*z1);


        for(int index = 0;index < cloud->points.size(); index++)
        {
            if(inliers.count(index) > 0)
                continue;

            pcl::PointXYZ point = cloud->points[index];
            float x3 = point.x;
            float y3 = point.y;
            float z3 = point.z;
            //计算点到平面距离公式
            float d = fabs(a*x3+b*y3+c*z3+d)/sqrt(a*a+ b*b+c*c);

            if(d <= distanceTol)
                inliers.insert(index); //小于阈值，判定为内点
        }

        if(inliers.size() > inliersResult.size())
        {
            inliersResult = inliers; //返回最优结果
        }
    }
    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "Ransac took " << elapsedTime.count() << " milliseconds" << std::endl;

    return inliersResult;

}

结果如图 红色为障碍物，绿色为道路点
在课程的工程中，直接调用了PCL库中的RANSAC功能进行点云的分割。其原理与仿真一致。

在lidar障碍物检测工程中的对应部分

std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SegmentPlane(typename pcl::PointCloud<PointT>::Ptr cloud, int maxIterations, float distanceThreshold)
{
    // Time segmentation process
    auto startTime = std::chrono::steady_clock::now();

    // TODO:: Fill in this function to find inliers for the cloud.
    //过滤之后的点云数据
    //创建分割对象 -- 检测平面参数
    pcl::SACSegmentation<PointT> seg;
    pcl::PointIndices::Ptr inliers {new pcl::PointIndices};   //存储内点的点索引集合对象inliers
    pcl::ModelCoefficients::Ptr coefficients{new pcl::ModelCoefficients};

    seg.setOptimizeCoefficients(true);       //设置对估计的模型系数需要进行优化
    seg.setModelType(pcl::SACMODEL_PLANE);  //设置模型类型，检测平面
    seg.setMethodType(pcl::SAC_RANSAC);     //设置方法（随机样本一致性）
    seg.setMaxIterations(maxIterations);    //最大迭代次数
    seg.setDistanceThreshold(distanceThreshold);   //距离阈值

    seg.setInputCloud(cloud);
    seg.segment(*inliers,*coefficients);    //分割操作

    if(inliers->indices.size() == 0)
    {
        std::cout<<"could not estimate a planar model for the given dataset."<<std::endl;
    }


    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "plane segmentation took " << elapsedTime.count() << " milliseconds" << std::endl;

    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);
    return segResult;
}

这个函数在environment.cpp文件中被调用

 std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr,pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI.SegmentPlane(inputCloud,25,0.3)

inputCloud为输入点云，是经过滤波处理后的，20为最大迭代次数，0.3为判定阈值，小于阈值的点判定为内点。在SegmentPlane函数最后有这样一条语句

std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult = SeparateClouds(inliers,cloud);

这条语句中调用了SeparateClouds函数，这个函数也在processPointCloud.cpp文件中，函数功能是将提取点云并存为平面点云和目标物点云

std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::SeparateClouds(pcl::PointIndices::Ptr inliers, typename pcl::PointCloud<PointT>::Ptr cloud)
{
    // TODO: Create two new point clouds, one cloud with obstacles and other with segmented plane
    typename pcl::PointCloud<PointT>::Ptr obstCloud (new pcl::PointCloud<PointT> ()); //目标点云指针
    typename pcl::PointCloud<PointT>::Ptr planeCloud (new pcl::PointCloud<PointT> ());//平面点云指针

    for(int index: inliers->indices)
        planeCloud->points.push_back(cloud->points[index]);

    pcl::ExtractIndices<PointT> extract;  //ExtractIndices通过分割算法提取部分点云数据子集的下标索引
    extract.setInputCloud (cloud); //设置输入点云
    extract.setIndices(inliers); //提取内点
    extract.setNegative(true); //提取外点，继续处理
    extract.filter(*obstCloud);//将外点（即目标点）提取至obstCloud

    std::pair<typename pcl::PointCloud<PointT>::Ptr, typename pcl::PointCloud<PointT>::Ptr> segResult(obstCloud, planeCloud);//储存两种点云
    return segResult;
}

2.聚类Clustering
在对点云进行分割后，工程已经可以将lidar的返回点云数据分割成目标物（障碍物）点云和平面（车道平面）点云，接下来要完成的任务就是将障碍物的点云进行聚类，将属于一个障碍物的点云归为一类。课程中使用的聚类方法是欧氏距离聚类，这个算法是根据点之间的紧密程度来关联点组。 为了有效地进行最近邻居搜索，可以使用KD-Tree数据结构，该结构平均可以将查找时间从O（n）加快到O（log（n））。相关资料可以参考以下文章：
KD-Tree与欧式距离聚类
udacity的工程中使用的是PCL库的欧式聚类算法，但同样对采用KD-Tree结构进行欧式聚类的方法进行的简要介绍和仿真。这里不过多记录，直接看工程中的实现部分
在environment.cpp函数中下面语句调用了Clustering函数

std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI.Clustering(segmentCloud.first,0.53,10,500);  //障碍物点云，采用KdTree将其进行欧式聚类

Clustering函数有三个参数
参数1为输入点云，这里segmentCloud.first表示是障碍物点云，我们对障碍物点云进行聚类
参数2为设置欧式聚类的近邻搜索的搜索半径，半径越大，搜索范围越大
参数3、4为一个类中点的上限和下限

Clustering函数定义如下

std::vector<typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{

    // Time clustering process
    auto startTime = std::chrono::steady_clock::now();

    std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;

    // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
    typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);    //kd树，是一种分割k维数据空间的数据结构。

    tree->setInputCloud(cloud);  //输入点云

    std::vector<pcl::PointIndices> clusterIndices;   //找到簇的索引
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance(clusterTolerance);   //设置近邻搜索的搜索半径
    ec.setMinClusterSize(minSize); //设置一个类所需要的最少点
    ec.setMaxClusterSize(maxSize); //设置一个类点云的上限
    ec.setSearchMethod(tree);    //设置点云的搜索机制，此处为KdTree
    ec.setInputCloud(cloud);
    ec.extract(clusterIndices);     //从点云中提取聚类，并保存在点云索引clusterIndices中

    for(pcl::PointIndices getIndices : clusterIndices)
    {
        typename pcl::PointCloud<PointT>::Ptr cloudCluster (new pcl::PointCloud<PointT>);

        for(int index : getIndices.indices)
            cloudCluster->push_back(cloud->points[index]);

        cloudCluster->width = cloudCluster->points.size();
        cloudCluster->height = 1;
        cloudCluster->is_dense = true;

        clusters.push_back(cloudCluster);
    }
    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size() << " clusters" << std::endl;

    return clusters;     //返回聚类
}

拿聚类仿真的图片来说明以下效果吧，同样颜色的点被分为一类。这也是KD-Tree的数据结构

至此，点云的分割和聚类操作已经完成，为方便观看，接下来要对一类的点云进行修饰，用一个立方体将一类的点云数据框起来， 这部分代码相对简单，直接上代码

Box ProcessPointClouds<PointT>::BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster)
{

    // Find bounding box for one of the clusters
    PointT minPoint, maxPoint;
    pcl::getMinMax3D(*cluster, minPoint, maxPoint
    );

    Box box;
    box.x_min = minPoint.x;
    box.y_min = minPoint.y;
    box.z_min = minPoint.z;
    box.x_max = maxPoint.x;
    box.y_max = maxPoint.y;
    box.z_max = maxPoint.z;

    return box;
}

这部分在传感器融合的第一篇文章中也记录过，不再赘述。
最后的结果就是下面这样
绿色点为车道平面点云 障碍物的点云分别渲染成了不同颜色，并对一类的点云，用立方体进行包围。每一个立方体就代表一个障碍物。