PCL分割及欧式聚类方法

分割的理解：把点云划分一下，好像一堆沙子，分成了很多堆沙子，记住，分出来的还是沙子。所以需要后续的聚类处理。

1、分割地面和障碍物。

原图：(已经做完滤波和降采样)

分割地面：

上图对应代码：

    ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
    pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloud = pointProcessorI->loadPcd("../src/sensors/data/pcd/data_1/0000000000.pcd");

    // renderPointCloud(viewer, inputCloud, "inputCloud");
    
    pcl::PointCloud<pcl::PointXYZI>::Ptr filterCloud = pointProcessorI->FilterCloud(inputCloud, 0.3, Eigen::Vector4f (-50, -5, -2, 1), Eigen::Vector4f (50, 7, 2, 1));
    // renderPointCloud(viewer, filterCloud, "filterCloud");

    std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI->RansacPlane(filterCloud, 20, 0.2);
    // renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1,0,0));
    renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0,1,0));//分割出的地面

分割障碍物:

上图对应代码：

    ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
    pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloud = pointProcessorI->loadPcd("../src/sensors/data/pcd/data_1/0000000000.pcd");

    // renderPointCloud(viewer, inputCloud, "inputCloud");
    
    pcl::PointCloud<pcl::PointXYZI>::Ptr filterCloud = pointProcessorI->FilterCloud(inputCloud, 0.3, Eigen::Vector4f (-50, -5, -2, 1), Eigen::Vector4f (50, 7, 2, 1));
    // renderPointCloud(viewer, filterCloud, "filterCloud");

    std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr, pcl::PointCloud<pcl::PointXYZI>::Ptr> segmentCloud = pointProcessorI->RansacPlane(filterCloud, 20, 0.2);
    renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1,0,0));//分割出的地表障碍物
    // renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0,1,0));//分割出的地面

具体算法采用Ransac 算法： 拟合平面-》取内点-》符合要求-》用内点重新拟合-》，不符合要求，再重新拟合，取内点最大的参数，即获得合适分割模型。

拟合的算法可以用最小二乘

template<typename PointC>
std::pair<typename pcl::PointCloud<PointC>::Ptr, typename pcl::PointCloud<PointC>::Ptr> ProcessPointClouds<PointC>::RansacPlane(typename pcl::PointCloud<PointC>::Ptr cloud, int maxIterations, float distanceTol)
{
	std::unordered_set<int> inliersResult;
	srand(time(NULL));

	// 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--)
	{
		// Select 3 points randomly
		std::unordered_set<int> inliers; // hash set
		while(inliers.size() != 3)
			inliers.insert(rand()%(cloud->points.size()));
		
		float x1, x2, x3, y1, y2, y3, z1, z2, z3;

		auto itr = inliers.begin();
		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;		

		std::vector<float> v1 = {x2-x1, y2-y1, z2-z1};
		std::vector<float> v2 = {x3-x1, y3-y1, z3-z1};

		std::vector<float> cross_v1v2 = {v1[1]*v2[2]-v1[2]*v2[1], v1[2]*v2[0]-v1[0]*v2[2], v1[0]*v2[1]-v1[1]*v2[0]};

		float a = cross_v1v2[0];
		float b = cross_v1v2[1];
		float c = cross_v1v2[2];
		float d = -(cross_v1v2[0]*x1 + cross_v1v2[1]*y1+cross_v1v2[2]*z1);

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

			PointC point = cloud->points[index];
			float x4 = point.x;
			float y4 = point.y;
			float z4 = point.z;

			float dist = fabs(a*x4+b*y4+c*z4+d)/sqrt(a*a+b*b+c*c);

			if(dist <= distanceTol)
				inliers.insert(index);

		}

		if (inliers.size() > inliersResult.size()) {
			inliersResult = inliers;
		}
        
	}

    //Convert unordered_set into pairs
    typename pcl::PointCloud<PointC>::Ptr  cloudInliers(new pcl::PointCloud<PointC>());
    typename pcl::PointCloud<PointC>::Ptr cloudOutliers(new pcl::PointCloud<PointC>());

    for(int i = 0; i < cloud->points.size(); i++)
    {
        PointC point = cloud->points[i];
        if(inliersResult.count(i))
            cloudInliers->points.push_back(point);
        else
            cloudOutliers->points.push_back(point);
    }

    std::pair<typename pcl::PointCloud<PointC>::Ptr, typename pcl::PointCloud<PointC>::Ptr> segResult(cloudOutliers, cloudInliers);

	return segResult;
}

2、聚类

聚类的理解：对分割出的"一堆一堆的沙子"进一步处理，实际上计算机不知道哪个沙子是"一堆"，只不过我们看上去分的很清楚，聚类的作用是让计算机知道哪个沙子是"一堆的"，也就是由很"多堆沙子"转换成"很多个目标级"输出。具体的算法使用欧式聚类

欧式聚类代码：具体过程很清晰，把点一个一个拿出来，计算空间距离，这里用到KDtree，它加快了检索速度！

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

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

    // Setup KdTree
    KdTree* tree = new KdTree;
    std::vector<std::vector<float>> points;
    
    int i = 0;
    for (auto point : cloud->points)
    {
        const std::vector<float> p{point.x, point.y, point.z};
        tree->insert(p, i++);
        points.push_back(p);
    }

    // Setup the Cluster
    std::vector<std::vector<int>> clusters_indices = euclideanCluster(points, tree, clusterTolerance);

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

    for(auto indices : clusters_indices)
    {
        if (indices.size() < minSize || indices.size() > maxSize) {continue;}
        
        typename pcl::PointCloud<PointC>::Ptr clusterCloud(new pcl::PointCloud<PointC>());
        
        for(auto index : indices)
        {
            clusterCloud->points.push_back(cloud->points[index]);
        }
        
        clusterCloud->width = clusterCloud->points.size();
        clusterCloud->height = 1;
        clusterCloud->is_dense = true;
        
        clusters.push_back(clusterCloud);
    }
    
    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;
}

聚类结果：返回的框，算法返回的是一团一团的子点云，其实还是点云，但是返回的vector就是分出来的点云，根据每一团画框。

    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters = pointProcessorI->Clustering_Custom(segmentCloud.first, 0.4, 10, 500);
    //聚类算法返回的是一团一团的子点云
    int clusterId = 0;
    std::vector<Color> colors = {Color(1,0,0), Color(1,1,0), Color(0,0,1)};

    for(pcl::PointCloud<pcl::PointXYZI>::Ptr cluster : cloudClusters)
    {
        std::cout << "cluster size ";
        pointProcessorI->numPoints(cluster);
        renderPointCloud(viewer, cluster, "obstCloud"+std::to_string(clusterId), colors[clusterId%3]);  // fix bug of color id
        //根据这一团一团的点云，画框
        Box box = pointProcessorI->BoundingBox(cluster);
        renderBox(viewer, box, clusterId);

        ++clusterId;
    }

画框代码：

template<typename PointC>
Box ProcessPointClouds<PointC>::BoundingBox(typename pcl::PointCloud<PointC>::Ptr cluster)
{
    // Find bounding box for one of the clusters
    PointC 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;
}