一、采样一致性算法
1.1 概述
1.2 目的和意义
目的:用于排除错误的样本,样本不同对应的应用则不同,例如剔除错误的配准点对、分割出处在模型上的点集等。
1.3 方法总结
在计算机视觉领域广泛应用各种不同的采样一致性参数估计算法 PCL 中以随机采样一致性算法(RANSAC)为核心,同时实现了五种类似于随机采样一致性估计参数算法的随机参数估计算法,例如随机采样一致性估计(RANSAC)、最大似然一致性估计(MLESAC) 最小中值方差一致性估计(LMEDS)等,所有的估计参数算法都符合一致性准则。
(1)RANSAC的原理讲解
(a) RANSAC的引入【引用参考文献2】
RANSAC是“RANdom Sample Consensus(随机抽样一致)”的缩写。
它可以从一组包含“局外点”的观测数据集中,通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果;
为了提高概率必须提高迭代次数。
数据分两种:有效数据(inliers)和 无效数据(outliers)。
偏差不大的数据称为有效数据,偏差大的数据是无效数据。
如果有效数据占大多数,无效数据只是少量时,我们可以通过最小二乘法或类似的方法来确定模型的参数和误差;
如果无效数据很多(比如超过了50%的数据都是无效数据),最小二乘法就 失效了,我们需要新的算法。
所以我们提出了RANSAC算法。下面通过一个例子来介绍。
一个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点,其中局内点近似的被直线所通过,而局外点远离于直线。
简单的最小二乘法不能找到适应于局内点的直线,原因是最小二乘法尽量去适应包括局外点在内的所有点。
相反,RANSAC能得出一个仅仅用局内点计算出模型,并且概 率还足够高。但是,RANSAC并不能保证结果一定正确,为了保证算法有足够高的合理概率,我们必须小心的选择算法的参数。包含很多局外点的数据集 RANSAC找到的直线(局外点并不影响结果)
(b)RANSAC算法
RANSAC算法的输入是一组观测数据,一个可以解释或者适应于观测数据的参数化模型,一些可信的参数。
RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点
并用下述方法进行验证:
- 有一个模型适应于假设的局内点,即所有的未知参数都能从假设的局内点计算得出。
- 用1中得到的模型去测试所有的其它数据,如果某个点适用于估计的模型,认为它也是局内点。
- 如果有足够多的点被归类为假设的局内点,那么估计的模型就足够合理。
- 然后,用所有假设的局内点去重新估计模型,因为它仅仅被初始的假设局内点估计过。
- 最后,通过估计局内点与模型的错误率来评估模型。
(c)伪代码
输入
data —— 一组观测数据
model —— 适应于数据的模型
n —— 适用于模型的最少数据个数
k —— 算法的迭代次数
t —— 用于决定数据是否适应于模型的阀值
d —— 判定模型是否适用于数据集的数据数目
best_model —— 跟数据最匹配的模型参数(如果没有找到好的模型,返回null)
best_consensus_set —— 估计出模型的数据点
best_error —— 跟数据相关的估计出的模型错误
输出
开始:
iterations = 0
best_model = null
best_consensus_set = null
best_error = 无穷大
while ( iterations < k )
maybe_inliers = 从数据集中随机选择n个点
maybe_model = 适合于maybe_inliers的模型参数
consensus_set = maybe_inliers
for ( 每个数据集中不属于maybe_inliers的点 )
if ( 如果点适合于maybe_model,且错误小于t )
将点添加到consensus_set
if ( consensus_set中的元素数目大于d )
已经找到了好的模型,现在测试该模型到底有多好
better_model = 适合于consensus_set中所有点的模型参数
this_error = better_model究竟如何适合这些点的度量
if ( this_error < best_error )
我们发现了比以前好的模型,保存该模型直到更好的模型出现
best_model = better_model
best_consensus_set = consensus_set
best_error = this_error
增加迭代次数
返回 best_model, best_consensus_set, best_error
(2)最大似然一致性估计(MLESAC)
(a)算法描述
(3) 最小中值法LMEDS
(a)算法描述
LMedS的做法很简单,就是从样本中随机抽出N个样本子集,使用最大似然(通常是最小二乘)对每个子集计算模型参数和该模型的偏差,记录该模型参数及子集中所有样本中偏差居中的那个样本的偏差(即Med偏差),
最后选取N个样本子集中Med偏差最小的所对应的模型参数作为我们要估计的模型参数。
但是与RANSAC不同的是,LMedS记录的是所有样本中偏差值居中的那个样本的偏差,称为Med偏差(这也是LMedS中Med的由来),以及本次计算得到的模型参数。
由于这一变化,LMedS不需要预先设定阅值来区分inliers和outliers。重复前面的过程N次,从中N个Med偏差中挑选出最小的一个,其对应的模型参数就是最终的模型参数估计值。其中迭代次数N是由样本子集中样本的个数、期望的模型误差、事先估计的样本中outliers的比例所决定。
LMedS理论上也可以剔除outliers的影响,并得到全局最优的参数估计,而且克服了RANSAC的两个缺点(虽然 LMedS也需要实现设定样本中outliers的比例,
1.4 实验部分
(1)使用采样采样一致化
采样一致化的步骤:
1. SampleConsensus (const SampleConsensusModelPtr &model, double threshold, bool random=false)
其中model设置随机采样性算法使用的模型,threshold 阀值
2.设置模型 void setSampleConsensusModel (const SampleConsensusModelPtr &model)
3.设置距离阈值 void setDistanceThreshold (double threshold)
4.获取距离阈值 double getDistanceThreshold ()
5.设置最大迭代次数 void setMaxIterations (int max_iterations)
6.获取最大迭代次数 int getMaxIterations ()
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h> // 可视化
#include <pcl/filters/extract_indices.h>//按索引提取点云
#include <boost/make_shared.hpp>
int
main(int argc, char** argv)
{
// 新建点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 随机生成点云
cloud->width = 15;
cloud->height = 1;//无序点云
cloud->points.resize(cloud->width * cloud->height);
// 生成
for (size_t i = 0; i < cloud->points.size(); ++i)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1.0;//都在z=1平面上
}
// 设置一些外点 , 即重新设置几个点的z值,使其偏离z为1的平面
cloud->points[0].z = 2.0;
cloud->points[3].z = -2.0;
cloud->points[6].z = 4.0;
// 打印点云信息
std::cerr << "Point cloud data: " << cloud->points.size() << " points" << std::endl;
for (size_t i = 0; i < cloud->points.size(); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
// 模型系数
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//内点索引
// pcl::PointIndices::Ptr outliers (new pcl::PointIndices);//外点索引
// 创建一个点云分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 是否优化模型系数
seg.setOptimizeCoefficients(true);
// 设置模型 和 采样方法
seg.setModelType(pcl::SACMODEL_PLANE);// 平面模型
seg.setMethodType(pcl::SAC_RANSAC);// 随机采样一致性算法
seg.setDistanceThreshold(0.01);//是否在平面上的阈值
seg.setInputCloud(cloud);//输入点云
seg.segment(*inliers, *coefficients);//分割 得到平面系数 已经在平面上的点的 索引
// seg.setNegative(true);//设置提取内点而非外点
//seg.segment(*inliers, *coefficients);//分割 得到平面系数 已经在平面上的点的 索引
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
// 打印平面系数
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
//打印平面上的点
std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
for (size_t i = 0; i < inliers->indices.size(); ++i)
std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " "
<< cloud->points[inliers->indices[i]].y << " "
<< cloud->points[inliers->indices[i]].z << std::endl;
// 3D点云显示 源点云 绿色
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(255, 255, 255);//背景颜色 白色
//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler(cloud, 1.0, 1.0, 0.0);
//viewer.addPointCloud (cloud, color_handler, "raw point");//添加点云
//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw point");
//viewer.addCoordinateSystem (1.0);
viewer.initCameraParameters();
//按照索引提取点云 内点
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;//索引提取器
extract_indices.setIndices(boost::make_shared<const pcl::PointIndices>(*inliers));//设置索引
extract_indices.setInputCloud(cloud);//设置输入点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
extract_indices.filter(*output);//提取对于索引的点云 内点
std::cerr << "output point size : " << output->points.size() << std::endl;
//平面上的点云 红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_handler(output, 255, 0, 0);
viewer.addPointCloud(output, output_handler, "plan point");//添加点云
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "plan point");
// 外点 绿色
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other(new pcl::PointCloud<pcl::PointXYZ>);
// *cloud_other = *cloud - *output;
// 移去平面局内点,提取剩余点云
extract_indices.setNegative(true);
extract_indices.filter(*cloud_other);
std::cerr << "other point size : " << cloud_other->points.size() << std::endl;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_other_handler(cloud_other, 0, 255, 0);
viewer.addPointCloud(cloud_other, cloud_other_handler, "other point");//添加点云
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "other point");
while (!viewer.wasStopped()) {
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(1000));
}
return (0);
}
运行结果
./random_sample_consensus.exe -s,可以看出菱形中有一部分是局外点
./random_sample_consensus.exe -f 展示菱形,包括了局外点和局内点
./random_sample_consensus.exe -sf 展示1/4球
(2) 使用采样一致化分割平面
/*
平面模型分割, 基于随机采样一致性
*/
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h> // 可视化
#include <pcl/filters/extract_indices.h>//按索引提取点云
#include <boost/make_shared.hpp>
int
main(int argc, char** argv)
{
// 新建点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 随机生成点云
cloud->width = 100;
cloud->height = 1;//无序点云
cloud->points.resize(cloud->width * cloud->height);
// 生成
for (size_t i = 0; i < cloud->points.size(); ++i)
{
cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
cloud->points[i].z = 1.0;//都在z=1平面上
}
// 设置一些外点 , 即重新设置几个点的z值,使其偏离z为1的平面
cloud->points[0].z = 20.0;
cloud->points[3].z = -20.0;
cloud->points[6].z = 40.0;
cloud->points[10].z = 40.0;
cloud->points[20].z = 40.0;
// 打印点云信息
std::cerr << "Point cloud data: " << cloud->points.size() << " points" << std::endl;
for (size_t i = 0; i < cloud->points.size(); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
// 模型系数
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);//内点索引
// pcl::PointIndices::Ptr outliers (new pcl::PointIndices);//外点索引
// 创建一个点云分割对象
pcl::SACSegmentation<pcl::PointXYZ> seg;
// 是否优化模型系数
seg.setOptimizeCoefficients(true);
// 设置模型 和 采样方法
seg.setModelType(pcl::SACMODEL_PLANE);// 平面模型
seg.setMethodType(pcl::SAC_RANSAC);// 随机采样一致性算法
seg.setDistanceThreshold(0.01);//是否在平面上的阈值
seg.setInputCloud(cloud);//输入点云
seg.segment(*inliers, *coefficients);//分割 得到平面系数 已经在平面上的点的 索引
// seg.setNegative(true);//设置提取内点而非外点
//seg.segment(*inliers, *coefficients);//分割 得到平面系数 已经在平面上的点的 索引
if (inliers->indices.size() == 0)
{
PCL_ERROR("Could not estimate a planar model for the given dataset.");
return (-1);
}
// 打印平面系数
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
//打印平面上的点
std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
for (size_t i = 0; i < inliers->indices.size(); ++i)
std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " "
<< cloud->points[inliers->indices[i]].y << " "
<< cloud->points[inliers->indices[i]].z << std::endl;
// 3D点云显示 源点云 绿色
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(255, 255, 255);//背景颜色 白色
//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color_handler(cloud, 1.0, 1.0, 0.0);
//viewer.addPointCloud (cloud, color_handler, "raw point");//添加点云
//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "raw point");
//viewer.addCoordinateSystem (1.0);
viewer.initCameraParameters();
//按照索引提取点云 内点
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;//索引提取器
extract_indices.setIndices(boost::make_shared<const pcl::PointIndices>(*inliers));//设置索引
extract_indices.setInputCloud(cloud);//设置输入点云
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
extract_indices.filter(*output);//提取对于索引的点云 内点
std::cerr << "output point size : " << output->points.size() << std::endl;
//平面上的点云 红色
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> output_handler(output, 255, 0, 0);
viewer.addPointCloud(output, output_handler, "plan point");//添加点云
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "plan point");
// 外点 绿色
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_other(new pcl::PointCloud<pcl::PointXYZ>);
// *cloud_other = *cloud - *output;
// 移去平面局内点,提取剩余点云
extract_indices.setNegative(true);
extract_indices.filter(*cloud_other);
std::cerr << "other point size : " << cloud_other->points.size() << std::endl;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_other_handler(cloud_other, 0, 255, 0);
viewer.addPointCloud(cloud_other, cloud_other_handler, "other point");//添加点云
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 8, "other point");
while (!viewer.wasStopped()) {
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return (0);
}
运行结果,这五个绿色点则是局外点,按照平面进行切割后,展示的结果。
二、参考补充
1、SIFT补充(这篇博客包含了SIFT的原理介绍以及代码演示,还有资料总结,很值得看)
2、PCL 点云分割与分类(这篇博客讲到了RANSAC的原理和各种分割方法原理)
3、RANSAC的讲解(这篇博客中详细说明了内点和外点,还有关于算法的描述)