移除离群点------PCL

文章介绍了使用PCL库中的statisticalOutlierRemoval、RadiusOutlierRemoval和ConditionalRemoval三种方法对点云数据进行离群点检测和移除的操作,涉及设置参数和条件比较等技术细节。

statisticalOutlierRemoval滤波器移除离群点

/// <summary>
/// 使用statisticalOutlierRemoval滤波器移除离群点
/// </summary>
/// <param name="cloud">被过滤的点云</param>
/// <param name="meank"></param>
/// <param name="threshold"></param>
/// <param name="Inversion">是否对结果取反,false:删除离群点,true:保留离群点</param>
/// <returns></returns>
pcl::PointCloud<pcl::PointXYZ>::Ptr PclTool::statisticalOutlierRemovalFilter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, int meank, double threshold, bool inversion)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;  // 创建滤波器对象

    sor.setInputCloud(cloud);     // 设置待滤波的点云
    sor.setMeanK(meank);          // 设置在进行统计时考虑查询点临近点数
    sor.setStddevMulThresh(1.0);  // 设置判断是否为离群点的阀值
    sor.setNegative(inversion);
    sor.filter(*cloud_filtered);  // 存储

    return cloud_filtered;
}
  • 删除离群点
    在这里插入图片描述
  • 仅保留离群点
    在这里插入图片描述

RadiusOutlinerRemoval 移除离群点

/// <summary>
/// RadiusOutlinerRemoval 移除离群点
/// </summary>
/// <param name="cloud"></param>
/// <param name="radius">设置半径的范围内找临近点</param>
/// <param name="minInRadius">设置查询点的邻域点集数小于minInRadius的删除</param>
/// <returns></returns>
pcl::PointCloud<pcl::PointXYZ>::Ptr PclTool::RORemoval(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, double radius, int minInRadius)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;  // 创建滤波器

    outrem.setInputCloud(cloud);        // 设置输入点云
    outrem.setRadiusSearch(radius);     // 设置半径为0.8的范围内找临近点
    outrem.setMinNeighborsInRadius(minInRadius);  // 设置查询点的邻域点集数小于2的删除
    // apply filter
    outrem.filter(*cloud_filtered);  // 执行条件滤波   在半径为0.8 在此半径内必须要有两个邻居点,此点才会保存
    
    return cloud_filtered;
}

在这里插入图片描述

ConditionalRemoval 移除离群点

pcl::PointCloud<pcl::PointXYZ>::Ptr PclTool::conditionRemoval(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<pcl::FieldComparison<pcl::PointXYZ>::ConstPtr> comparisons)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

    // 创建条件限定的下的滤波器
    pcl::ConditionAnd<pcl::PointXYZ>::Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>());  // 创建条件定义对象

    // 添加在Z字段上大于0的比较算子
    // GT greater than
    // EQ equal
    // LT less than
    // GE greater than or equal
    // LE less than  or equal 小于等于

    //// 为条件定义对象添加比较算子
    //pcl::FieldComparison<pcl::PointXYZ>::ConstPtr comp1(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0));     // 添加在Z字段上大于0的比较算子
    //pcl::FieldComparison<pcl::PointXYZ>::ConstPtr comp2(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8));     // 添加在Z字段上小于0.8的比较算子
    //range_cond->addComparison(comp1);
    //range_cond->addComparison(comp2);


    //range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::GT, 0.0)));  // 添加在Z字段上大于0的比较算子

    //range_cond->addComparison(pcl::FieldComparison<pcl::PointXYZ>::ConstPtr(new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps::LT, 0.8)));  // 添加在Z字段上小于0.8的比较算子

    for (const auto& once : comparisons)
    {
        range_cond->addComparison(once);
    }
    
    // 创建滤波器并用条件定义对象初始化
    pcl::ConditionalRemoval<pcl::PointXYZ> condrem;
    condrem.setCondition(range_cond);
    condrem.setInputCloud(cloud);    // 输入点云
    condrem.setKeepOrganized(true);  // 设置保持点云的结构
    // 设置是否保留滤波后删除的点,以保持点云的有序性,通过setuserFilterValue设置的值填充点云;或从点云中删除滤波后的点,从而改变其组织结构
    // 如果设置为true且不设置setUserFilterValue的值,则用nan填充点云
    // https://blog.youkuaiyun.com/qq_37124765/article/details/82262863

    // 执行滤波
    condrem.filter(*cloud_filtered);

    return cloud_filtered;
}
<think>我们讨论的是在ROS环境中将ROS的PointCloud2消息转换为PCL点云对象(使用python-pcl库)的方法。 步骤: 1. 订阅ROS的PointCloud2话题。 2. 使用pcl_helper(通常来自pcl_ros)或自己编写转换函数将PointCloud2消息转换为PCL点云。 3. 然后就可以使用python-pcl库的功能处理点云。 注意:python-pcl库是PCL(Point Cloud Library)的Python绑定,允许在Python中使用PCL的功能。 我们将提供: - 一个ROS节点示例,该节点订阅PointCloud2话题,将其转换为PCL点云,然后可以执行一些处理(如下采样),最后可能再转换回ROS消息发布。 由于pcl_helper不是标准安装的,我们可以提供转换函数。 转换函数的主要思路: - 从PointCloud2消息中提取点云数据,并构造为numpy数组,然后转换为pcl.PointCloud对象。 另外,注意python-pcl的安装:可以通过pip安装(如`pip install python-pcl`),但需要系统安装PCL库。 示例代码:</think>### Python-PCL库中ROS PointCloud2转PCL点云完整指南 #### 核心转换方法与实现 ##### 1. 基本转换函数 ```python import pcl import numpy as np import rospy from sensor_msgs.msg import PointCloud2 from sensor_msgs import point_cloud2 def ros_to_pcl(ros_cloud): """将ROS PointCloud2消息转换为PCL点云对象""" # 获取点云数据 points_list = [] for data in point_cloud2.read_points( ros_cloud, field_names=("x", "y", "z", "rgb"), skip_nans=True ): points_list.append([data[0], data[1], data[2], data[3]]) # 创建PCL点云 pcl_cloud = pcl.PointCloud_PointXYZRGB() pcl_cloud.from_list(points_list) return pcl_cloud ``` ##### 2. 高效转换(使用NumPy) ```python def ros_to_pcl_numpy(ros_cloud): """高效转换(使用NumPy)""" # 直接获取点云数据为结构化数组 gen = point_cloud2.read_points(ros_cloud, skip_nans=True) np_data = np.array(list(gen), dtype=np.float32) # 创建PCL点云 pcl_cloud = pcl.PointCloud_PointXYZRGB() pcl_cloud.from_array(np_data[:, :4]) # 只取前4列 (x,y,z,rgb) return pcl_cloud ``` #### 完整ROS节点示例 ```python #!/usr/bin/env python import rospy import pcl import pcl_helper import numpy as np from sensor_msgs.msg import PointCloud2 from pcl_msgs.msg import PointIndices class RosPclConverter: def __init__(self): rospy.init_node('ros_pcl_converter') # 订阅原始点云话题 self.sub = rospy.Subscriber("/input_cloud", PointCloud2, self.cloud_callback) # 发布处理后的点云 self.pub_raw = rospy.Publisher("/pcl_cloud", PointCloud2, queue_size=1) self.pub_processed = rospy.Publisher("/processed_cloud", PointCloud2, queue_size=1) rospy.loginfo("ROS-PCL转换器已启动") def cloud_callback(self, ros_cloud): try: # 转换ROS消息为PCL点云 pcl_cloud = self.ros_to_pcl(ros_cloud) # 发布原始PCL点云(转回ROS) self.pub_raw.publish(pcl_helper.pcl_to_ros(pcl_cloud)) # 点云处理示例:体素网格下采样 processed_cloud = self.process_cloud(pcl_cloud) # 发布处理后的点云 self.pub_processed.publish(pcl_helper.pcl_to_ros(processed_cloud)) except Exception as e: rospy.logerr(f"点云处理错误: {str(e)}") def ros_to_pcl(self, ros_cloud): """优化版ROS到PCL转换""" # 获取点云数据 points = [] for data in point_cloud2.read_points(ros_cloud, skip_nans=True): # 处理RGB字段(如果存在) rgb = data[3] if len(data) > 3 else 0 # 支持不同点类型 if len(data) > 4: # 包含强度字段 points.append([data[0], data[1], data[2], rgb, data[4]]) else: points.append([data[0], data[1], data[2], rgb]) # 创建对应类型的点云 if len(points[0]) == 5: pcl_cloud = pcl.PointCloud_PointXYZIRGB() else: pcl_cloud = pcl.PointCloud_PointXYZRGB() pcl_cloud.from_list(points) return pcl_cloud def process_cloud(self, cloud): """点云处理管道示例""" # 1. 体素网格下采样 vox = cloud.make_voxel_grid_filter() vox.set_leaf_size(0.01, 0.01, 0.01) # 1cm体素 # 2. 移除离群点 sor = vox.filter().make_statistical_outlier_filter() sor.set_mean_k(50) sor.set_std_dev_mul_thresh(1.0) # 3. 平面分割 (提取桌面等平面) seg = sor.filter().make_segmenter() seg.set_model_type(pcl.SACMODEL_PLANE) seg.set_method_type(pcl.SAC_RANSAC) seg.set_distance_threshold(0.01) # 获取平面内点 inliers, coefficients = seg.segment() # 4. 提取平面上的物体 extract = cloud.make_extract_indices() extract.set_indices(inliers) extract.set_negative(True) # 提取非平面点 return extract.filter() if __name__ == '__main__': RosPclConverter() rospy.spin() ``` #### 高级功能实现 ##### 1. 点云类型自动检测 ```python def detect_cloud_type(ros_cloud): """根据ROS消息字段检测点云类型""" fields = {field.name for field in ros_cloud.fields} if 'intensity' in fields and 'rgb' in fields: return pcl.PointCloud_PointXYZIRGB elif 'intensity' in fields: return pcl.PointCloud_PointXYZI elif 'rgb' in fields: return pcl.PointCloud_PointXYZRGB else: return pcl.PointCloud_PointXYZ ``` ##### 2. 时间同步处理(多传感器融合) ```python import message_filters class MultiSensorProcessor: def __init__(self): # 创建时间同步器 cloud_sub = message_filters.Subscriber('/camera/cloud', PointCloud2) image_sub = message_filters.Subscriber('/camera/image', Image) ts = message_filters.ApproximateTimeSynchronizer( [cloud_sub, image_sub], queue_size=10, slop=0.1 ) ts.registerCallback(self.sync_callback) def sync_callback(self, cloud_msg, image_msg): # 转换点云 pcl_cloud = ros_to_pcl(cloud_msg) # 处理同步数据 self.process_synced_data(pcl_cloud, image_msg) ``` ##### 3. PCL到ROS转换函数 ```python def pcl_to_ros(pcl_cloud, frame_id='map'): """将PCL点云转换为ROS PointCloud2消息""" ros_cloud = PointCloud2() ros_cloud.header.stamp = rospy.Time.now() ros_cloud.header.frame_id = frame_id # 设置点云字段 fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] # 添加RGB或强度字段 if pcl_cloud.sensor_type in [pcl.PointCloud_PointXYZRGB, pcl.PointCloud_PointXYZIRGB]: fields.append(PointField('rgb', 12, PointField.FLOAT32, 1)) if pcl_cloud.sensor_type == pcl.PointCloud_PointXYZIRGB: fields.append(PointField('intensity', 16, PointField.FLOAT32, 1)) # 创建消息 ros_cloud = point_cloud2.create_cloud(ros_cloud.header, fields, pcl_cloud.to_list()) return ros_cloud ``` #### 性能优化技巧 1. **内存优化**: ```python # 使用预分配内存 def optimized_conversion(ros_cloud): # 获取点数量 point_count = ros_cloud.width * ros_cloud.height # 预分配NumPy数组 np_points = np.zeros((point_count, 4), dtype=np.float32) # 直接填充数组 index = 0 for data in point_cloud2.read_points(ros_cloud, skip_nans=True): if index >= point_count: break np_points[index] = [data[0], data[1], data[2], data[3] if len(data) > 3 else 0] index += 1 # 创建点云 cloud = pcl.PointCloud_PointXYZRGB() cloud.from_array(np_points) return cloud ``` 2. **多线程处理**: ```python from threading import Thread import queue class ProcessingPipeline: def __init__(self): self.cloud_queue = queue.Queue(maxsize=3) self.worker = Thread(target=self.process_worker) self.worker.daemon = True self.worker.start() def cloud_callback(self, ros_cloud): try: # 非阻塞放入队列 self.cloud_queue.put_nowait(ros_cloud) except queue.Full: rospy.logwarn("点云队列已满,丢弃帧") def process_worker(self): while not rospy.is_shutdown(): try: ros_cloud = self.cloud_queue.get(timeout=1.0) pcl_cloud = ros_to_pcl(ros_cloud) # 执行耗时处理... except queue.Empty: continue ``` #### 常见问题解决 1. **字段不匹配错误**: ```python # 字段兼容性处理 def handle_missing_fields(ros_cloud): # 检查是否存在rgb字段 has_rgb = any(f.name == 'rgb' for f in ros_cloud.fields) # 添加缺失的字段 if not has_rgb: # 创建新字段列表 new_fields = list(ros_cloud.fields) new_fields.append(PointField('rgb', 12, PointField.FLOAT32, 1)) # 创建新点云消息 new_cloud = PointCloud2() new_cloud.header = ros_cloud.header new_cloud.height = ros_cloud.height new_cloud.width = ros_cloud.width new_cloud.fields = new_fields new_cloud.is_bigendian = ros_cloud.is_bigendian new_cloud.point_step = ros_cloud.point_step + 4 # 增加4字节 new_cloud.row_step = ros_cloud.row_step + ros_cloud.width * 4 new_cloud.data = ros_cloud.data + b'\x00' * (ros_cloud.width * ros_cloud.height * 4) new_cloud.is_dense = ros_cloud.is_dense return new_cloud return ros_cloud ``` 2. **性能瓶颈分析**: ```python # 添加性能监控装饰器 def timing_decorator(func): def wrapper(*args, **kwargs): start = rospy.Time.now() result = func(*args, **kwargs) end = rospy.Time.now() rospy.loginfo(f"{func.__name__} 耗时: {(end - start).to_sec():.4f}秒") return result return wrapper # 应用装饰器 @timing_decorator def ros_to_pcl(ros_cloud): # 转换代码... ``` 3. **点云对齐问题**: ```python # 使用ICP进行点云配准 def align_point_clouds(source_cloud, target_cloud): """使用ICP配准两个点云""" icp = source_cloud.make_IterativeClosestPoint() converged, transf, estimate, fitness = icp.icp( source_cloud, target_cloud, max_iter=100 ) if converged: aligned_cloud = source_cloud.make_moving_least_squares() aligned_cloud.set_input_cloud(source_cloud) aligned_cloud.set_search_method(pcl.KdTreeFLANN()) aligned_cloud.process() return aligned_cloud return source_cloud ``` #### 安装与依赖 **系统要求**: ```bash # 安装依赖 sudo apt-get install ros-$ROS_DISTRO-pcl-conversions ros-$ROS_DISTRO-pcl-ros sudo apt-get install python-pcl python3-numpy # 安装python-pcl pip install python-pcl --user # 验证安装 python -c "import pcl; print(pcl.__version__)" ```
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值