PCL学习（5）随机采样一致性算法RANSAC

一、RANSAC概念及作用

1.1 基本概念

RANSAC是一种鲁棒的参数估计方法，用于从包含大量异常值的数据中拟合数学模型。其核心思想是通过随机采样和迭代验证，找到最优的模型参数，避免异常值的干扰。

1.2 核心思想

• 随机采样：每次从数据中随机选取最小子集（如拟合直线时选2个点）来估计模型参数。

• 一致性检验：用估计的模型测试其他数据点，统计符合模型的内点（inliers，误差小于阈值的数据）。

• 迭代优化：重复上述过程，选择内点最多的模型作为最优解。

1.2.1 算法步骤

1. 随机采样：从数据中随机选取最小样本集（如拟合平面需3个点）。

2. 模型估计：用采样点计算模型参数（如直线方程）。

3. 内点检测：计算所有数据点到模型的误差，统计满足误差阈值的点（内点）。

4. 评估模型：若当前内点比例高于历史最优，则更新模型和内点集合。

5. 终止条件：达到预设迭代次数或内点比例足够高时停止。

1.2.2 优缺点

• 优点：

  • 对噪声和异常值不敏感。

  • 无需数据预处理（如滤波）。

• 缺点：

  • 计算成本高（依赖迭代次数）。

  • 需预设阈值（如内点误差阈值）。

1.2.3 数学表达

1.3 PCL中的随机参数估计算法

PCL 中以随机采样一致性算法( RANSAC) 为核心，实现了五种类似于RANSAC的随机参数估计算法，例如随机采样一致性估计（RANSAC ) 、最大似然一致性估计 （MLESAC ) 、最小中值方差一致性估计 ( LMEDS )等，所有的估计参数算法都符合一致性准则。利用RANSAC可以实现点云分割，目前 PCL 中支持的几何模型分割有空间平面、直线、二维或三维圆、圆球、锥体等 。 

二、代码解析

2.1 生成噪声和球体面

	for (std::size_t i = 0; i < cloud->points.size(); ++i){cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);if (i % 5 == 0){cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);}else if (i % 2 == 0){cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)- (cloud->points[i].y * cloud->points[i].y));}else{cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x)- (cloud->points[i].y * cloud->points[i].y));}}

生成一个合成点云，其中包含：随机噪声点和一个隐含的球体表面点

比例	Z 坐标生成方式	几何意义
20%	完全随机 ([0, 1024))	离群点（噪声）
40%	z = +sqrt(1 - x² - y²)	单位球上半球面（内点）
40%	z = -sqrt(1 - x² - y²)	单位球下半球面（内点）

• 有效球面点：80% 的点分布在单位球上（x² + y² + z² = 1）。

• 噪声点：20% 的点是随机噪声，用于测试 RANSAC 的鲁棒性。

2.2 RANSAC算法过程

	std::vector<int> inliers;pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);ransac.setDistanceThreshold(.01);ransac.computeModel();ransac.getInliers(inliers);

2.2.1 创建球体模型对象

pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));

• 作用：定义一个 RANSAC 可拟合的球体模型。

• 参数：

  • pcl::PointXYZ：点云类型（包含 x, y, z 坐标）。

  • cloud：输入点云（pcl::PointCloud<pcl::PointXYZ>::Ptr 类型）。

• 输出：model_s 是球体模型的智能指针，用于后续 RANSAC 拟合。

2.2.2 初始化 RANSAC 算法

pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);

• 作用：创建RANSAC求解器，绑定球体模型 model_s。

• 关键点：

  • RANSAC 会从 cloud 中随机采样点，尝试拟合球体。

  • 适用于存在大量离群点（outliers）的情况。

2.2.3 设置距离阈值

ransac.setDistanceThreshold(0.01);

• 作用：定义内点（inliers）的判定条件。

• 参数：

  • 0.01：点到球面的最大允许距离（单位：米）。

  • 若点距球面距离 ≤ 0.01，则判定为内点。

• 影响：

  • 值越小，拟合越严格。

  • 值越大，鲁棒性越强。

2.2.4 执行RANSAC拟合

ransac.computeModel();

• 作用：运行 RANSAC 算法，迭代寻找最优球体参数。

• 内部流程：

  1. 随机采样最小点集（球体需 4 个点）。

  2. 计算球体参数（中心 (cx, cy, cz) 和半径 r）。

  3. 统计所有满足 distance ≤ 0.01 的内点。

  4. 重复迭代，保留内点最多的模型。

2.2.5 获取内点索引

ransac.getInliers(inliers);

• 作用：提取被 RANSAC 判定为内点的索引。

• 参数：

  • inliers：通常是 pcl::PointIndices::Ptr 或 std::vector<int> 类型，存储内点的索引。

2.2.6 copyPointCloud函数

pcl::copyPointCloud(*cloud, inliers, *final);

这行代码的作用是从原始点云 cloud 中提取 RANSAC 拟合后得到的内点（inliers），并将其复制到一个新的点云final中。

• 输入：

  • *cloud：原始点云。

  • indices：需要复制的点的索引（这里是 inliers，即 RANSAC 找到的内点）。

• 输出：

  • cloud_out：存储提取后的点云（这里是 *final）。

2.3 点云显示函数

pcl::visualization::PCLVisualizer::Ptr 
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, pcl::PointCloud<pcl::PointXYZ>::ConstPtr final = nullptr)
{pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");if (final != nullptr){pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler(final, 255, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(final, color_handler, "final cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "final cloud");}viewer->addCoordinateSystem(1.0, "global");viewer->initCameraParameters();return (viewer);
}

上述代码将原点云显示为白色，将筛选后的点云显示为红色。

2.4案例完整代码

2.2.1 案例球形估计

#include <iostream>
#include <thread>#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_parallel_plane.h>
#include <pcl/sample_consensus/sac_model_normal_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>pcl::visualization::PCLVisualizer::Ptr 
simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud, pcl::PointCloud<pcl::PointXYZ>::ConstPtr final = nullptr)
{pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");if (final != nullptr){pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler(final, 255, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(final, color_handler, "final cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "final cloud");}viewer->addCoordinateSystem(1.0, "global");viewer->initCameraParameters();return (viewer);
}int main(int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);cloud->width = 3000;cloud->height = 1;cloud->is_dense = false;cloud->points.resize(cloud->width * cloud->height);for (std::size_t i = 0; i < cloud->points.size(); ++i){cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);if (i % 5 == 0){cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);}else if (i % 2 == 0){cloud->points[i].z = sqrt(1 - (cloud->points[i].x * cloud->points[i].x)- (cloud->points[i].y * cloud->points[i].y));}else{cloud->points[i].z = -sqrt(1 - (cloud->points[i].x * cloud->points[i].x)- (cloud->points[i].y * cloud->points[i].y));}}std::vector<int> inliers;pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);ransac.setDistanceThreshold(100);ransac.computeModel();ransac.getInliers(inliers);pcl::copyPointCloud(*cloud, inliers, *final);pcl::visualization::PCLVisualizer::Ptr viewer;viewer = simpleVis(cloud, final);while (!viewer->wasStopped()){viewer->spinOnce(100);}return -1;
}

2.2.2 案例平面估计

#include <iostream>
#include <ctime>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/sample_consensus/sac_model_perpendicular_plane.h>
#include <pcl/sample_consensus/ransac.h>pcl::visualization::PCLVisualizer::Ptr simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr PointCloud,pcl::PointCloud<pcl::PointXYZ>::ConstPtr final = nullptr)
{pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);viewer->addPointCloud<pcl::PointXYZ>(PointCloud, "PointCloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "PointCloud");pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> final_point_color_handle(final, 255, 0, 0);viewer->addPointCloud(final, final_point_color_handle,"fianl");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, "fianl");viewer->addCoordinateSystem(200);viewer->initCameraParameters();return viewer;
}int main(int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZ>::Ptr PointCloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);PointCloud->width = 1500;PointCloud->height = 1;PointCloud->is_dense = false;PointCloud->points.resize(PointCloud->width * PointCloud->height);for (size_t i = 0; i < PointCloud->points.size(); ++i){PointCloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);PointCloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);if (i % 2 == 0){PointCloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);}else{PointCloud->points[i].z =0.5 * ( PointCloud->points[i].x + PointCloud->points[i].y);}}std::vector<int> inliers;pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(PointCloud));pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);ransac.setDistanceThreshold(0.001);ransac.computeModel();ransac.getInliers(inliers);pcl::copyPointCloud(*PointCloud, inliers, *final);pcl::visualization::PCLVisualizer::Ptr viewer;viewer = simpleVis(PointCloud, final);while (!viewer->wasStopped()){viewer->spinOnce(100);}return 0;
}