随机采样一致性算法实现

原理

随机采样一致性RANSAC (Random Sample Consensus) 是一种鲁棒的模型拟合算法。该算法通过随机采样和迭代优化，从包含噪声和离群点的数据中估计数学模型参数，常用于点云分割和平面拟合。

算法核心思想：

RANSAC通过迭代地从数据中随机选择最小样本集（如拟合平面需要3个点）来估计模型参数，然后根据距离阈值判断哪些点是内点（符合模型），最终选择具有最多内点的模型作为最优解。

算法详细流程

随机采样 ：从点云中随机选择拟合模型所需的最小样本数 mmm（平面：3个点）
模型估计 ：用这 mmm 个点计算模型参数 θ\thetaθ
内点判断 ：对于点云中所有点，计算到模型的距离 did_idi，如果 di<ϵd_i < \epsilondi<ϵ，则判定为内点
内点计数 ：统计符合模型的内点数量 ninliern_{\text{inlier}}ninlier
模型评估 ：如果当前模型的内点数 ninliern_{\text{inlier}}ninlier 大于历史最佳，则更新最佳模型
迭代终止 ：重复步骤1-5，直到达到最大迭代次数 NNN 或满足收敛条件
模型优化：用所有内点重新估计模型参数，得到最终模型

数学表达式

点 pip_ipi 到模型 M(θ)M(\theta)M(θ) 的距离：di=distance(pi,M(θ))d_i = \text{distance}(p_i, M(\theta))di=distance(pi,M(θ))
内点判断条件：di<ϵd_i < \epsilondi<ϵ
内点集合：I={pi∣di<ϵ}I = \{p_i | d_i < \epsilon\}I={pi∣di<ϵ}
目标函数：max⁡θ∣I(θ)∣\max_{\theta} |I(\theta)|maxθ∣I(θ)∣（最大化内点数量）

迭代次数计算

RANSAC所需的理论迭代次数 NNN 可以通过以下公式估计：
N=log⁡(1−p)log⁡(1−wm)N = \frac{\log(1 - p)}{\log(1 - w^m)}N=log(1−wm)log(1−p)

其中：

ppp：期望得到正确模型的概率（通常设为0.99）
www：数据中内点的比例（需要估计）
mmm：拟合模型所需的最小样本数

参数作用分析

距离阈值 (DistanceThreshold)：
- 判断点是否为内点的最大距离
- 过小：可能遗漏真正内点，导致模型拟合不准确
- 过大：可能包含太多外点，降低模型精度
- 推荐值：根据点云精度和噪声水平设置，通常为0.01-0.1米
最大迭代次数 (MaxIterations)：
- 算法最大运行迭代次数
- 过小：可能未找到最优模型
- 过大：计算时间增加，可能无必要
- 推荐值：1000-10000（根据数据复杂度调整）
概率阈值 (Probability)：
- 期望找到正确模型的概率
- 通常设为0.99（99%置信度）
- 值越高，需要的迭代次数越多
模型类型 (ModelType)：
- 要拟合的几何模型类型
- 常用类型：平面(SACMODEL_PLANE)、圆柱体(SACMODEL_CYLINDER)、球体(SACMODEL_SPHERE)、直线(SACMODEL_LINE)

算法特点

强鲁棒性：对离群点具有很好的容忍性
模型多样性：可以拟合多种几何模型
自适应迭代：根据内点比例自动调整迭代次数
计算复杂度 ：迭代次数与内点比例相关，复杂度为 O(N⋅k)O(N \cdot k)O(N⋅k)，其中 kkk 为每次迭代的计算量

算法优势

离群点鲁棒性：即使数据中包含大量离群点，也能准确拟合模型
模型通用性：适用于多种几何模型拟合
理论保证：有数学理论支持，可以计算所需迭代次数
参数直观：参数有明确的物理意义

算法局限性

计算效率：对于低内点比例的数据，需要大量迭代
模型假设：需要预先知道要拟合的模型类型
参数敏感性：距离阈值的选择对结果影响较大
局部最优：可能陷入局部最优解，特别是当数据中存在多个模型时

实现算法

基本用法:

cpp 复制代码

pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);      // 优化模型系数
seg.setModelType(pcl::SACMODEL_PLANE);  // 设置平面模型
seg.setMethodType(pcl::SAC_RANSAC);     // 使用RANSAC方法
seg.setMaxIterations(1000);             // 设置最大迭代次数
seg.setDistanceThreshold(0.01);         // 设置内点距离阈值

seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);   // 执行分割得到内点和模型系数

// 提取内点作为滤波结果
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers);
extract.setNegative(false);             // false:提取内点，true:提取外点
extract.filter(*cloud_filtered);

多种模型类型支持:

cpp 复制代码

// 平面模型（最小样本数：3）
seg.setModelType(pcl::SACMODEL_PLANE);

// 圆柱体模型（最小样本数：7，需要法线）
seg.setModelType(pcl::SACMODEL_CYLINDER);

// 球体模型（最小样本数：4）
seg.setModelType(pcl::SACMODEL_SPHERE);

// 直线模型（最小样本数：2）
seg.setModelType(pcl::SACMODEL_LINE);

// 圆环模型（最小样本数：5）
seg.setModelType(pcl::SACMODEL_CIRCLE2D);

// 圆锥模型（最小样本数：6）
seg.setModelType(pcl::SACMODEL_CONE);

高级参数设置:

cpp 复制代码

// 设置概率阈值（默认为0.99）
seg.setProbability(0.99);

// 设置半径限制（对于圆柱体、球体等模型）
seg.setRadiusLimits(0.1, 0.5);  // 最小半径0.1m，最大半径0.5m

// 设置轴限制（对于圆柱体模型）
Eigen::Vector3f axis(0, 0, 1);  // Z轴方向
seg.setAxis(axis);
seg.setEpsAngle(pcl::deg2rad(15.0));  // 角度容差15度

完整代码

cpp 复制代码

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>

int main() {
    // 1. 加载点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (pcl::io::loadPCDFile<pcl::PointXYZ>("02.pcd", *cloud) == -1) {
        std::cerr << "错误：无法加载02.pcd文件" << std::endl;
        return -1;
    }
    
    std::cout << "加载点云成功，点数: " << cloud->points.size() << std::endl;
    
    // 2. 创建分割对象
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);      // 优化模型系数
    seg.setModelType(pcl::SACMODEL_PLANE);  // 平面模型
    seg.setMethodType(pcl::SAC_RANSAC);     // RANSAC方法
    seg.setMaxIterations(1000);             // 最大迭代次数
    seg.setDistanceThreshold(0.01);         // 内点距离阈值
    
    // 3. 执行RANSAC分割
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
    
    if (inliers->indices.size() == 0) {
        std::cerr << "错误：未找到平面模型" << std::endl;
        return -1;
    }
    
    std::cout << "RANSAC平面拟合结果:" << std::endl;
    std::cout << "  内点数量: " << inliers->indices.size() << std::endl;
    std::cout << "  外点数量: " << cloud->size() - inliers->indices.size() << std::endl;
    std::cout << "  平面方程: " 
              << coefficients->values[0] << "x + "
              << coefficients->values[1] << "y + "
              << coefficients->values[2] << "z + "
              << coefficients->values[3] << " = 0" << std::endl;
    
    // 4. 提取内点作为拟合结果
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
    
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(false);             // 提取内点（平面上的点）
    extract.filter(*filtered);
    
    std::cout << "RANSAC滤波后点数: " << filtered->points.size() << std::endl;
    std::cout << "参数: 模型=平面, 距离阈值=0.01m, 最大迭代=1000" << std::endl;
    
    // 5. 可视化
    pcl::visualization::PCLVisualizer viewer("filter");
    
    // 原始点云（红色，半透明）
    viewer.addPointCloud<pcl::PointXYZ>(cloud, "original");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "original");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "original");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.3, "original");
    
    // 滤波后点云（白色）- 内点
    viewer.addPointCloud<pcl::PointXYZ>(filtered, "filtered");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 1.0, "filtered");
    viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "filtered");

    while (!viewer.wasStopped()) {
        viewer.spinOnce(100);
    }

    return 0;
}

运行结果

用两种不同的数据进行平面拟合，当数据中包含平面时，能够完全拟合,选取的其中最大的一个平面进行拟合。