ubuntu20.04下C++实现点云的多边形区域过滤(2种实现：1、pcl的CropHull滤波器；2、CUDA上实现射线法)

在点云目标检测中，经常会有一系列的误识别，为了减小误识别的概率，可以通过区域过滤来删除不需要的点云，如下图所示
在这里插入图片描述
本例中点云的场景为路口交通场景，已经把雷达坐标系的xoy面转换至点云中的地平面，具体原理和操作参考：https://blog.csdn.net/m0_64293675/article/details/145208953?spm=1001.2014.3001.5502
这样可以将区域的划分从3维变成2维，只需要给出多边形区域的X和Y坐标，z轴无限延伸即可。
区域过滤的第一种方法是使用PCL库的CropHull滤波器，也就是常说的凸包算法，第二种方法是射线法，即从点出发画一条射线（例如水平向右），统计该射线与多边形边的交点个数。如果交点个数为奇数，点在多边形内；偶数，则在多边形外。

C++实现点云的多边形区域过滤的2种方法

1、使用PCL库的CropHull滤波器
2、射线法的CUDA实现

1、使用PCL库的CropHull滤波器

需要安装pcl库，apt安装即可，建议安装版本大于等于1.8，ubuntu20.04用ap安装的pcl默认版本是1.10.0，符合要求。如果想从源码安装，自行baidu。

sudo apt install libpcl-dev

cut_roi.cpp

#include <iostream>
#include <vector>
#include <chrono>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/crop_hull.h>
#include <pcl/surface/convex_hull.h>using namespace std;// 不需要intensity的可以用typedef pcl::PointXYZ PointT;
typedef pcl::PointXYZI PointT;// 删除一个多边形区域内的点
// cloud 输入点云
// polygon 多边形区域的顶点坐标
pcl::PointCloud<PointT>::Ptr removePointsInPolygon(pcl::PointCloud<PointT>::Ptr cloud,const vector<pair<float, float>> &polygon)
{// 创建多边形点云pcl::PointCloud<PointT>::Ptr hullPoints(new pcl::PointCloud<PointT>);for (const auto &vertex : polygon){// 根据PointT 的内容对应赋值PointT p;p.x = vertex.first;p.y = vertex.second;p.z = 0.0f;p.intensity = 0.0f;hullPoints->push_back(p);}// 添加第一个点使多边形闭合hullPoints->push_back(hullPoints->points[0]);// 创建凸包对象pcl::ConvexHull<PointT> hull;hull.setInputCloud(hullPoints);hull.setDimension(2); // 2D多边形// 存储凸包结果的容器pcl::PointCloud<PointT>::Ptr hullCloud(new pcl::PointCloud<PointT>);vector<pcl::Vertices> hullPolygons;hull.reconstruct(*hullCloud, hullPolygons);// 创建裁剪对象pcl::CropHull<PointT> cropFilter;cropFilter.setHullCloud(hullCloud);cropFilter.setHullIndices(hullPolygons);cropFilter.setDim(2); // 2D操作// 执行裁剪（保留多边形外的点）pcl::PointCloud<PointT>::Ptr filteredCloud(new pcl::PointCloud<PointT>);cropFilter.setInputCloud(cloud);cropFilter.setCropOutside(false); // 保留多边形外部的点，删除内部的点cropFilter.filter(*filteredCloud);return filteredCloud;
}// 删除多个多边形区域内的点
pcl::PointCloud<PointT>::Ptr removePointsInPolygons(pcl::PointCloud<PointT>::Ptr cloud,const vector<vector<pair<float, float>>> &polygons)
{for (const auto &polygon : polygons){cloud = removePointsInPolygon(cloud, polygon);}return cloud;
}int main()
{using clock = chrono::high_resolution_clock;using ms = chrono::milliseconds;using ns = chrono::nanoseconds;pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);//while (1){pcl::io::loadPCDFile("../test.pcd", *cloud);//示例多区域的顶点(x,y) 顺时针逆时针都可可以vector<vector<pair<float, float>>> polygons = {{{79.5060272217, 79.175064086},{79.2747802734, 58.571964263},{81.6638717651, 41.394405365},{86.0577163696, 30.583795547},{101.274185181, 24.103635788},{122.022064209, 20.140935897},{149.628707886, 18.349731445},{195.621643066, 9.6359605789},{198.321121216, 69.765792846},{138.335754395, 130.86445617}},{{77.3776473999, -91.604118347},{80.9268569946, -45.400650024},{109.261535645, -20.841529846},{158.885635376, -19.598711013},{219.02204895, -22.8725452423},{188.727401733, -147.86555481},{159.747970581, -147.89741516},{126.374259949, -128.46434021}},{{59.590133667, 55.9770889282},{59.0997047424, 34.932685852},{54.9787826538, 34.951171875},{54.4650382996, 52.371749877}}};auto start1 = clock::now();// 删除多边形区域内的点pcl::PointCloud<PointT>::Ptr filteredCloud = removePointsInPolygons(cloud, polygons);auto end1 = clock::now();ns duration1 = chrono::duration_cast<ns>(end1 - start1);cout << "CPU上 使用pcl库的CropHull滤波器删除多个多边形区域内的点 耗时: " << duration1.count() / 1000000.0 << " 毫秒" << endl;// 保存结果// pcl::io::savePCDFile("../filtered.pcd", *filteredCloud);cout << "num_input: " << cloud->size() << endl;cout << "num_output: " << filteredCloud->size() << endl;}return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(lidar_process)add_definitions(-std=c++11)set(CMAKE_CXX_STANDARD 11)set(CMAKE_BUILD_TYPE Release)
# set(CMAKE_BUILD_TYPE Debug)
cmake_policy(SET CMP0074 NEW)find_package(PCL)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})include_directories("include/")add_executable(lidar_process src/cut_roi.cpp)target_link_libraries(lidar_process ${PCL_LIBRARIES})

编译运行

 mkdir buildcd buildcmake ..make./lidar_process

2、射线法的CUDA实现

需要安装合适版本的CUDA，可以参考：https://blog.csdn.net/m0_64293675/article/details/141166292?spm=1001.2014.3001.5502

cut_roi_kernel.cuh

#ifndef CUT_LIDAR_ROI_CUH
#define CUT_LIDAR_ROI_CUH#include <iostream>
#include <vector>
#include <cuda_runtime.h>
#include <thrust/device_vector.h>
#include <thrust/host_vector.h>// 点云数据结构
struct Point_cuda
{float x;float y;float z;
};// 多边形信息结构体
struct PolygonInfo {int start_idx;      // 在顶点数组中的起始索引int num_vertices;   // 多边形的顶点数int is_negative;    // 0=将该区域删除, 1=将该区域保留
};// CUDA核函数：判断点是否在单个多边形内,若返回true 表示点在多边形内
__device__ bool isPointInSinglePolygon(float px, float py, const float2* polygon, int num_vertices);// CUDA核函数：标记点是否在任何一个多边形内
__global__ void markPointsInPolygonsKernel(const float3* points, const float2* all_vertices, const PolygonInfo* polygons_info,int num_points, int num_polygons, int* flags);// CUDA核函数：压缩点云（保留flags[idx]标记为0的点，不保留flags[idx]标记为1的点）
__global__ void compactPointsKernel(const float3* points, const int* flags, int num_points, float3* output, int* output_index);// CUDA实现点云多区域裁剪
std::vector<Point_cuda>  removePointsInPolygonsCUDA(std::vector<Point_cuda>  cloud, const std::vector<std::vector<std::pair<float,float>>>& polygons,const std::vector<int>& polygon_types);
#endif

cut_roi_kernel.cu

#include "cut_roi_kernel.cuh"// CUDA错误检查宏
#define CHECK_CUDA_ERROR(call) \
do { \cudaError_t err = call; \if (err != cudaSuccess) { \std::cerr << "CUDA error at " << __FILE__ << ":" << __LINE__ \<< " - " << cudaGetErrorString(err) << std::endl; \exit(EXIT_FAILURE); \} \
} while(0)// CUDA核函数：判断点是否在单个多边形内,若返回true 表示点在多边形内
__device__ bool isPointInSinglePolygon(float px, float py, const float2* polygon, int num_vertices) 
{int crossings = 0;for (int i = 0; i < num_vertices; i++) {int next = (i + 1) % num_vertices;float2 v1 = polygon[i];float2 v2 = polygon[next];// 检查射线是否穿过边if (((v1.y <= py) && (v2.y > py)) || ((v1.y > py) && (v2.y <= py))) {float x = v1.x + (py - v1.y) * (v2.x - v1.x) / (v2.y - v1.y);if (x > px) {crossings++;}}}// 奇数交点表示在多边形内return (crossings % 2 == 1);
}// CUDA核函数：标记点是否在任何一个多边形内
__global__ void markPointsInPolygonsKernel(const float3* points, const float2* all_vertices, const PolygonInfo* polygons_info,int num_points, int num_polygons, int* flags) 
{int idx = blockIdx.x * blockDim.x + threadIdx.x;if (idx >= num_points) return;float px = points[idx].x;float py = points[idx].y;// 初始化标记为0（点不在任何多边形内）int inside_any = 0;// 遍历所有多边形for (int p = 0; p < num_polygons; p++) {PolygonInfo poly_info = polygons_info[p];const float2* poly_vertices = &all_vertices[poly_info.start_idx];// 检查点是否在当前多边形内bool inside_current = isPointInSinglePolygon(px, py, poly_vertices, poly_info.num_vertices);// 根据多边形类型更新标记if (inside_current) {// 点在多边形内，如果is_negative == 0，表示要将该区域内的点删除，需要将inside_any标志记为1//如果is_negative == 1，表示要将该区域内的点保留，inside_any标志不变(仍为0)if (poly_info.is_negative == 0) {inside_any = 1;break; // 点在一个区域，无需检查其他多边形}        }}flags[idx] = inside_any;
}// CUDA核函数：压缩点云（保留flags[idx]标记为0的点，不保留flags[idx]标记为1的点）
__global__ void compactPointsKernel(const float3* points, const int* flags, int num_points, float3* output, int* output_index) 
{int idx = blockIdx.x * blockDim.x + threadIdx.x;if (idx >= num_points) return;// 获取输出索引if (flags[idx] == 0) {int pos = atomicAdd(output_index, 1);output[pos] = points[idx];}
}// CUDA实现点云多区域裁剪
std::vector<Point_cuda>  removePointsInPolygonsCUDA(std::vector<Point_cuda>  cloud, const std::vector<std::vector<std::pair<float,float>>>& polygons,const std::vector<int>& polygon_types) 
{// 1. 准备数据int num_points = cloud.size();int num_polygons = polygons.size();// 2. 准备主机端数据结构thrust::host_vector<float3> h_points(num_points);thrust::host_vector<float2> h_all_vertices;thrust::host_vector<PolygonInfo> h_polygons_info(num_polygons);// 转换点云数据for (int i = 0; i < num_points; i++) {h_points[i] = make_float3(cloud[i].x, cloud[i].y, cloud[i].z);}std::cout<<"num_input:"<<num_points<<std::endl;// 构建多边形顶点数组和索引信息int vertex_offset = 0;for (int p = 0; p < num_polygons; p++) {const auto& poly = polygons[p];int num_vertices = poly.size();h_polygons_info[p].start_idx = vertex_offset;h_polygons_info[p].num_vertices = num_vertices;h_polygons_info[p].is_negative = polygon_types[p];for (const auto& vertex : poly) {h_all_vertices.push_back(make_float2(vertex.first, vertex.second));}vertex_offset += num_vertices;}// 3. 分配设备内存thrust::device_vector<float3> d_points = h_points;thrust::device_vector<float2> d_all_vertices = h_all_vertices;thrust::device_vector<PolygonInfo> d_polygons_info = h_polygons_info;thrust::device_vector<int> d_flags(num_points, 0);// 4. 创建输出索引thrust::device_vector<int> d_output_index(1, 0);// 5. 配置核函数参数dim3 blockSize(256);dim3 gridSize((num_points + blockSize.x - 1) / blockSize.x);// 6. 执行标记核函数markPointsInPolygonsKernel<<<gridSize, blockSize>>>(thrust::raw_pointer_cast(d_points.data()),thrust::raw_pointer_cast(d_all_vertices.data()),thrust::raw_pointer_cast(d_polygons_info.data()),num_points,num_polygons,thrust::raw_pointer_cast(d_flags.data()));CHECK_CUDA_ERROR(cudaDeviceSynchronize());// 7. 统计需要保留的点数thrust::host_vector<int> h_flags = d_flags;int num_inside = thrust::count(h_flags.begin(), h_flags.end(), 1);int num_outside = num_points - num_inside;// 8. 分配输出内存thrust::device_vector<float3> d_output(num_outside);// 重置输出索引thrust::fill(d_output_index.begin(), d_output_index.end(), 0);// 9. 执行压缩核函数compactPointsKernel<<<gridSize, blockSize>>>(thrust::raw_pointer_cast(d_points.data()),thrust::raw_pointer_cast(d_flags.data()),num_points,thrust::raw_pointer_cast(d_output.data()),thrust::raw_pointer_cast(d_output_index.data()));CHECK_CUDA_ERROR(cudaDeviceSynchronize());// 10. 复制结果回主机thrust::host_vector<float3> h_output = d_output;std::cout<<"num_output:"<<num_outside<<std::endl;// 11. 赋值std::vector<Point_cuda> filteredCloud;for (int i = 0; i < num_outside; i++) {Point_cuda tmp;tmp.x = h_output[i].x;tmp.y = h_output[i].y;tmp.z = h_output[i].z;filteredCloud.emplace_back(tmp);}return filteredCloud;
}

main.cpp

#include <iostream>
#include <chrono>
#include <vector>
#include <cmath>
#include <algorithm>
#include <fstream>
#include <sstream>
#include "cut_roi_kernel.cuh"
#include <cuda_runtime.h>using namespace std;
// 读取离线点云.bin文件, 这里避免用pcl库，以免还需安装pcl，当然也可以使用pcl的io读取.pcd格式的点云文件。
vector<Point_cuda> read_bin_files(string filename)
{vector<Point_cuda> pc_data_;std::ifstream file(filename, std::ios::binary);if (!file){std::cerr << "无法打开文件: " << filename << std::endl;return pc_data_;}// 读取文件直到文件末尾while (file.peek() != EOF){Point_cuda point;// 依次读取 x, y, z 坐标file.read(reinterpret_cast<char *>(&point.x), sizeof(float));file.read(reinterpret_cast<char *>(&point.y), sizeof(float));file.read(reinterpret_cast<char *>(&point.z), sizeof(float));pc_data_.push_back(point);}file.close();return pc_data_;
}// 保存删掉区域后的点云文件为txt，cloud compare软件也可以可视化.txt格式的点云文件
void write_all_pc_to_file(string filename, vector<Point_cuda> all_Point)
{// 打开文件以写入数据std::ofstream outFile(filename);// 检查文件是否成功打开if (outFile.is_open()){// 遍历 vector 并将每个元素写入文件for (auto num : all_Point){outFile << num.x << "," << num.y << "," << num.z << std::endl;}// 关闭文件outFile.close();std::cout << "数据已成功保存到" << filename << "文件。" << std::endl;}else{std::cerr << "无法打开文件。" << std::endl;}
}int main()
{using clock = chrono::high_resolution_clock;using ms = chrono::milliseconds;using ns = chrono::nanoseconds;// while (1){vector<Point_cuda> test_data = read_bin_files("../test.bin");//示例多区域的顶点(x,y) 顺时针逆时针都可可以vector<vector<pair<float, float>>> polygons = {{{79.5060272217, 79.175064086},{79.2747802734, 58.571964263},{81.6638717651, 41.394405365},{86.0577163696, 30.583795547},{101.274185181, 24.103635788},{122.022064209, 20.140935897},{149.628707886, 18.349731445},{195.621643066, 9.6359605789},{198.321121216, 69.765792846},{138.335754395, 130.86445617}},{{77.3776473999, -91.604118347},{80.9268569946, -45.400650024},{109.261535645, -20.841529846},{158.885635376, -19.598711013},{219.02204895, -22.8725452423},{188.727401733, -147.86555481},{159.747970581, -147.89741516},{126.374259949, -128.46434021}},{{59.590133667, 55.9770889282},{59.0997047424, 34.932685852},{54.9787826538, 34.951171875},{54.4650382996, 52.371749877}}};std::vector<int> polygon_types; // 0=将该区域删除, 1=将该区域保留for(int i = 0; i < polygons.size(); i++){polygon_types.push_back(0);}auto start1 = clock::now();vector<Point_cuda> filteredCloud = removePointsInPolygonsCUDA(test_data, polygons, polygon_types);auto end1 = clock::now();ns duration1 = chrono::duration_cast<ns>(end1 - start1);cout << "CUDA 射线法删除多个多边形区域内的点 耗时: " << duration1.count() / 1000000.0 << " 毫秒" << endl;write_all_pc_to_file("../filtered.txt", filteredCloud);}return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(lidar_process)add_definitions(-std=c++11)set(CMAKE_CXX_STANDARD 11)set(CMAKE_BUILD_TYPE Release)
# set(CMAKE_BUILD_TYPE Debug)
cmake_policy(SET CMP0074 NEW)# find_package(PCL)
# include_directories(${PCL_INCLUDE_DIRS})
# link_directories(${PCL_LIBRARY_DIRS})
# add_definitions(${PCL_DEFINITIONS})find_package(CUDA)
include_directories(${CUDA_INCLUDE_DIRS})# 根据自己的cuda路径和版本修改
include_directories("/usr/local/cuda-11.8/targets/x86_64-linux/include""/usr/local/cuda/include""/usr/local/include")
link_directories("/usr/local/cuda/lib64""/usr/local/cuda/targets/x86_64-linux/lib""/usr/local/lib")include_directories("include/")cuda_add_executable(lidar_processsrc/cut_roi_kernel.cusrc/main.cpp
)

编译运行

 mkdir buildcd buildcmake ..make./lidar_process