【C++】 pcl库中的CropHull滤波和凸包算法（convex hull）

最新推荐文章于 2024-07-10 15:53:26 发布

Zhang_Chen_

最新推荐文章于 2024-07-10 15:53:26 发布

阅读量6.5k

点赞数 6

分类专栏： C++

本文链接：https://blog.csdn.net/zhang_chen_/article/details/102417129

版权

C++ 专栏收录该内容

9 篇文章 5 订阅

订阅专栏

【C++】 pcl库中的CropHull滤波和凸包算法（convex hull）

部分参考来源
凸包算法解决什么问题
穷举法
分治法
Jarvis步进法
Graham扫描法
GropHull任意多边形内部点云提取
结语

部分参考来源

https://www.cnblogs.com/dream-it-possible/p/8514706.html

凸包算法解决什么问题

凸包（Convex Hull）是一个计算几何（图形学）中的概念。
在一个实数向量空间 $V$ 中，对于给定集合 $X$ ，所有包含 $X$ 的凸集的交集 $S$ 被称为 $X$ 的凸包。 $X$ 的凸包可以用 $X$ 内所有点 $X_1, ...X_n)$ 的凸组合来构造。

在二维欧几里得空间中，凸包可想象为一条刚好包着所有点的橡皮圈。

用不严谨的话来讲，给定二维平面上的点集，凸包就是将最外层的点连接起来构成的凸多边形，它能包含点集中所有的点。

凸包问题：给定点集，求构成凸包的点

穷举法

时间复杂度：O(n³）。
思路：两点确定一条直线，如果剩余的其它点都在这条直线的同一侧，则这两个点是凸包上的点，否则就不是。
步骤：

将点集里面的所有点两两配对，组成 $C_n^2 = n(n-1)/2$ 条直线
对于每条直线，再检查剩余的 $(n - 2)$ 个点是否在直线的同一侧

如何判断一个点 $p_3$ 是在点 $p_1、p_2$ 连成的直线的左边还是右边呢？坐标： $p_1(x_1, y_1)，p_2(x_2, y_2)，p_3(x_3, y_3)$
在这里插入图片描述

分治法

时间复杂度：O(n㏒n)。
思路：应用分治法思想，把一个大问题分成几个结构相同的子问题，把子问题再分成几个更小的子问题……。然后我们就能用递归的方法，分别求这些子问题的解。最后把每个子问题的解“组装”成原来大问题的解。
步骤：

把所有的点都放在二维坐标系里面。那么横坐标最小和最大的两个点 $P_1$ 和 $P_n$ 一定是凸包上的点。直线 $P_1P_n$ 把点集分成了两部分，即 $X$ 轴上面和下面两部分，分别叫做上包和下包
对上包：求距离直线 $P_1P_n$ 最远的点，假设为点 $P_{max}$
作直线 $P_1P_{max}$ 、 $P_nP_{max}$ ，把直线 $P_1P_{max}$ 左侧的点当成是上包，把直线 $P_nP_{max}$ 右侧的点也当成是上包
重复步骤 2、3
对下包也作类似操作

在这里插入图片描述
然而怎么求距离某直线最远的点呢？
设有一个点 $P_3$ 和直线 $P_1P_2$ 。坐标： $p_1(x_1, y_1)，p_2(x_2, y_2)，p_3(x_3, y_3)$

注意：在步骤一，如果横坐标最小的点不止一个，那么这几个点都是凸包上的点，此时上包和下包的划分就有点不同了，需要注意

#include <iostream>
#include <vector>

using namespace std;

vector<vector<int>> convex_hull; /*convex_hull储存所有凸包点*/

/*GetResult()实现功能：以坐标P0(x1,y1)和Pn(x2,y2)为直线，找出pack里面里这条直线最远的点Pmax
并找出直线P0Pmax和PmaxPn的上包，进行递归
*/

void GetResult(vector<vector<int>> point, int x1, int y1, int x2, int y2)
{
    /*tmax：最远点在point中的索引
    Rmax：最远距离的值*/
    int i, x3, y3, R, Rmax, tmax; 
    vector<vector<int>> result_pack; /*存放上包点或者下包点*/
    
    /*上包点或者下包点计数，初始化为零*/
    result_pack.push_back({0});
    
	x3 = point[1][0];
    y3 = point[1][1];
    R = x1*y2 + x3*y1 + x2*y3 - x3*y2 - x2*y1 - x1*y3;
    Rmax = R;
    tmax = 1;

    if (R >= 0)
    {
        result_pack.push_back({x3, y3});
        result_pack[0][0] = result_pack[0][0] + 1;
    }
    
    for(int i=2;i<=point[0][0];i++) /*从点集的第二个点开始循环*/
    {
        x3 = point[i][0];
        y3 = point[i][1];
        R = x1*y2 + x3*y1 + x2*y3 - x3*y2 - x2*y1 - x1*y3;
        if(R >= 0) /*如果R>=0，则是同一测包（上包或下包）的点*/
        {
            result_pack.push_back({x3, y3});
            result_pack[0][0] = result_pack[0][0] + 1;
        }
        if(R > Rmax)
        {
            Rmax = R;
            tmax = i;
        }
    } /*找到一测距离直线最远的点的距离和索引*/
    
    if(Rmax <= 0) /*如果已经是边界点了*/
    {
        for(int i=1;i<=result_pack[0][0];i++)
        {
            x3 = result_pack[i][0];
            y3 = result_pack[i][1];
            R = x1*y2 + x3*y1 + x2*y3 - x3*y2 - x2*y1 - x1*y3;
            if(R == 0 && !((x3==x2&&y3==y2)||(x3==x1&&y3==y1))) /*如果R是零并且这个新点不是决定直线的两个点，则加入凸包点集合*/
            {
                convex_hull.push_back({result_pack[i][0], result_pack[i][1]});
                convex_hull[0][0] = convex_hull[0][0] + 1;
            }
        }
        return;
    }
    else
    {
        convex_hull.push_back({point[tmax][0], point[tmax][1]});
        convex_hull[0][0] = convex_hull[0][0] + 1;
        if(result_pack[0][0] == 0)
            return;
    }
    GetResult(result_pack, x1, y1, point[tmax][0], point[tmax][1]);
    GetResult(result_pack, point[tmax][0], point[tmax][1], x2, y2);
}

int main(int argc, char** argv)
{
    vector<vector<int>> pointset; /*pointset储存所有点*/
    int count=1; /*整型变量conut用于计数*/
    int x1, y1, x2, y2, x3, y3; /*三个点的坐标*/
    convex_hull.push_back({0}); /*convex_hull的第一行第一列元素存放凸包点的个数，初始化为0*/
	pointset.push_back({0}); /*pointset的第一行第一列元素存放点集里面有几个点，初始化为0*/
    
	cout<<"===请输入所有点的坐标==="<<endl;
    
    /*初始化点集*/
    int x, y;
    while(count<20) /*设置输入20个点*/
	{
		cout<<"请输入点"<<count<<"的x轴坐标："<<endl;
		cin>>x;
		cout<<"请输入点"<<count<<"的y轴坐标："<<endl;
		cin>>y;
		pointset.push_back({x, y});
        count++;
	}
    /*点集里一共有多少个点*/
	pointset[0][0] = count-1; 

	x1 = pointset[1][0];
    y1 = pointset[1][1];
    
	x2 = x1;
    y2 = y1;
    
	for(int i=2;i<=pointset[0][0];i++)
    {
        x3 = pointset[i][0];
        y3 = pointset[i][1];
        if(x3 < x1)
        {
            x1 = x3;
            y1 = y3;
        } /*找到x最小的点赋给（x1, y1）*/
        else if(x3 > x2)
        {
            x2 = x3;
            y2 = y3;
        } /*找到x最大的点赋给（x2, y2）*/
    }

	/*两点是凸包点*/
    convex_hull.push_back({x1, y1});
    convex_hull.push_back({x2, y2});
	
	/*凸包点个数加二*/
    convex_hull[0][0] += 2;
    
    /*因为新x1-x2和x2-x1符号相反，所以上包点和下包点对应的“计算距离公式分子绝对值内的数学表达式”的一正一负
    所以下面调换x1和x2顺序作为输入保证两者计算的“计算距离公式分子绝对值内的数学表达式”为正的情况各是上包点和下包点中的一种*/
	GetResult(pointset, x1, y1, x2, y2);
    GetResult(pointset, x2, y2, x1, y1);

    /*打印凸包点*/
    cout<<"\n\n构成凸包的点有："<<endl;
    for(int i=1;i<=convex_hull[0][0];i++)
    {
        cout<<"("<<convex_hull[i][0]<<", "<<convex_hull[i][1]<<")"<<endl;
    }
}

Jarvis步进法

时间复杂度：O(nH)。（其中 n 是点的总个数，H 是凸包上的点的个数）
思路：纵坐标最小的那个点一定是凸包上的点，例如下图中的 $P_0$ 。从 $P_0$ 开始，按逆时针的方向，逐个找凸包上的点，每前进一步找到一个点，所以叫作步进法。

怎么找下一个点呢？利用夹角。假设现在已经找到 ${P_0，P_1，P_2}$ 了，要找下一个点：剩下的点分别和 $P_2$ 组成向量，设这个向量与向量 $P_1P_2$ 的夹角为 β 。当 β 最小时就是所要求的下一个点了，此处为 $P_3$ 。

注意：

找第二个点 $P_1$ 时，因为已经找到的只有 $P_0$ 一个点，所以向量只能和水平线作夹角 α，当 α 最小时求得第二个点
共线情况：如果直线 $P_2P_3$ 上还有一个点 $P_4$ ，即三个点共线，此时由向量 $P_2P_3$ 和向量 $P_2P_4$ 产生的两个 β 是相同的。我们应该把 $P_3$ 、 $P_4$ 都当做凸包上的点，并且把距离 $P_2$ 最远的那个点（即上图中的 $P_4$ ）作为最后搜索到的点，继续找它的下一个连接点

Graham扫描法

时间复杂度：O(n㏒n) 。
思路：Graham扫描的思想和Jarris步进法类似，也是先找到凸包上的一个点，然后从那个点开始按逆时针方向逐个找凸包上的点，但它不是利用夹角
步骤：

把所有点放在二维坐标系中，则纵坐标最小的点一定是凸包上的点，记为 $P_0$
把所有点的坐标平移一下，使 $P_0$ 作为原点
计算各个点相对于 $P_0$ 的幅角 α ，按从小到大的顺序对各个点排序。当 α 相同时，距离 $P_0$ 比较近的排在前面。例如上图得到的结果为 $P_1，P_2，P_3，P_4，P_5，P_6，P_7，P_8$ 。我们由几何知识可以知道，结果中第一个点 $P_1$ 和最后一个点 $P_8$ 一定是凸包上的点。以上，我们已经知道了凸包上的第一个点 $P_0$ 和第二个点 $P_1$ ，我们把它们放在栈里面。现在从步骤3求得的那个结果里，把 $P_1$ 后面的那个点拿出来做当前点，即 $P_2$ 。接下来开始找第三个点：
连接栈最上面的两个元素，得到直线 $L$ 。看当前点是在直线 $L$ 的右边还是左边。如果在直线的右边就执行步骤5；如果在直线上，或者在直线的左边就执行步骤6
如果在右边，则栈顶的那个元素不是凸包上的点，把栈顶元素出栈。执行步骤4
当前点是凸包上的点，把它压入栈，执行步骤7
检查当前的点是不是步骤3那个结果的最后一个元素。是最后一个元素的话就结束。如果不是的话就把当前点后面那个点做当前点，返回步骤4

最后，栈中的元素就是凸包上的点了

GropHull任意多边形内部点云提取

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <vector>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/concave_hull.h>

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PCDReader reader;
	reader.read(argv[1],*cloud); /*第一个参数是点云文件路径*/

	/*设置滤波的边框点*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr boundingbox_ptr (new pcl::PointCloud<pcl::PointXYZ>);
	boundingbox_ptr->push_back(pcl::PointXYZ(0.1, 0.1, 0));
	boundingbox_ptr->push_back(pcl::PointXYZ(0.1, -0.1,0 ));
	boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, 0.1,0 ));
	boundingbox_ptr->push_back(pcl::PointXYZ(-0.1, -0.1,0 ));
	boundingbox_ptr->push_back(pcl::PointXYZ(0.15, 0.1,0 ));

	/*求上面给出的这个边框点集的凸包*/
	pcl::ConvexHull<pcl::PointXYZ> hull;
	hull.setInputCloud(boundingbox_ptr);
	hull.setDimension(2); /*设置凸包维度*/
	std::vector<pcl::Vertices> polygons; /*用于保存凸包顶点*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr surface_hull (new pcl::PointCloud<pcl::PointXYZ>); /*用于描绘凸包形状*/
	hull.reconstruct(*surface_hull, polygons);

	pcl::PointCloud<pcl::PointXYZ>::Ptr objects (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::CropHull<pcl::PointXYZ> bb_filter;
	bb_filter.setDim(2); /*设置维度*/
	bb_filter.setInputCloud(cloud);
	bb_filter.setHullIndices(polygons); /*封闭多边形顶点*/
	bb_filter.setHullCloud(surface_hull); /*封闭多边形形状*/
	bb_filter.filter(*objects); /*结果保存到objects*/
	std::cout << objects->size() << std::endl;

	/*可视化结果*/
	boost::shared_ptr<pcl::visualization::PCLVisualizer> for_visualizer_v (new pcl::visualization::PCLVisualizer ("crophull display"));
	for_visualizer_v->setBackgroundColor(0,0,0);

	int v1(0);
	for_visualizer_v->createViewPort (0.0, 0.0, 0.33, 1, v1);
	for_visualizer_v->setBackgroundColor (0, 0, 0, v1); /*背景设置为黑色*/
	for_visualizer_v->addPointCloud (cloud,"cloud",v1);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloud"); /*点云设置为红色*/
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
	for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull, 0, .069*255,0.2*255, "backview_hull_polyline1",v1);

	int v2(0);
	for_visualizer_v->createViewPort (0.33, 0.0, 0.66, 1, v2);	
	for_visualizer_v->setBackgroundColor (1, 1, 1, v2); /*背景设置为白色*/
	for_visualizer_v->addPointCloud (surface_hull,"surface_hull",v2);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "surface_hull"); /*用红色可视化凸包形状*/
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"surface_hull");
	for_visualizer_v->addPolygon<pcl::PointXYZ>(surface_hull, 0, .069*255,0.2*255, "backview_hull_polyline",v2);

	int v3(0);
	for_visualizer_v->createViewPort (0.66, 0.0, 1, 1, v3);
	for_visualizer_v->setBackgroundColor (0, 0, 0, v3); /*背景设置为黑色*/
	for_visualizer_v->addPointCloud (objects,"objects",v3);
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "objects"); /*用红色可视化滤波后的点云*/
	for_visualizer_v->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"objects");

	while (!for_visualizer_v->wasStopped())
	{
		for_visualizer_v->spinOnce(1000);
	}
	system("pause");
}