padeoe的小站

RANSAC

• RANSAC（Random Sample Consensus），即随机采样一致性算法。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。该算法最早由Fischler和Bolles于1981年提出。
• 输入：一个数据集（点云），一个适用的模型（平面模型），一个可靠性参数（距离阈值）

算法过程

RANSAC通过反复选择数据中的一组随机子集来达成目标。被选取的子集被假设为局内点，并用下述方法进行验证：

1. 有一个模型适应于假设的局内点，即所有的未知参数都能从假设的局内点计算得出。
2. 用1中得到的模型去测试所有的其它数据，如果某个点适用于估计的模型，认为它也是局内点。
3. 如果有足够多的点被归类为假设的局内点，那么估计的模型就足够合理。
4. 然后，用所有假设的局内点去重新估计模型，因为它仅仅被初始的假设局内点估计过。
5. 最后，通过估计局内点与模型的错误率来评估模型。

这个过程被重复执行固定的次数，每次产生的模型要么因为局内点太少而被舍弃，要么因为比现有的模型更好而被选用。

平面模型的估计过程

1.输入：

1. 数据：点云
2. 模型：平面模型
3. 可靠性参数：距离阈值

2.用于估计的系数:

平面方程：ax+by+cz+d=0,系数a,b,c,d

3.迭代过程

• 随机选取三个点假设为内点，确定平面方程系数，计算其余点与该平面的距离，根据距离阈值判断是否满足估计的模型，将满足的加入内点，并计算内点个数
• 将假设的内点重新估计模型系数
• 重复执行以上两个步骤，若内点个数太少，将抛弃该模型，重新执行以上过程，若产生比现有模型更好的模型，则抛弃旧模型。反复迭代固定次数，得到最终模型

PCL应用

1.第一步：输入点云

[cpp]
#include <pcl/sample_consensus/method_types.h> //随机参数估计方法的头文件
#include <pcl/sample_consensus/model_types.h>//模型的定义，这里是平面模型
#include <pcl/segmentation/sac_segmentation.h>//采样一致性算法的具体方法
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 设置点云属性，为随机点云，共15个点
cloud->width = 15;
cloud->height = 1;
cloud->points.resize(cloud->width * cloud->height);
// 随机设置点云的坐标，其中x，y值随机产生，z值恒为1，因此是z=1平面上散布的点
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;
}

// 改变其中几个点的位置，使之脱离平面成为外点
cloud->points[0].z = 2.0;
cloud->points[3].z = -2.0;
cloud->points[6].z = 4.0;
[/cpp]

回显输入的点云数据

[cpp]
std::cerr << "初始点云数据（共 "
<< cloud->points.size() << " 个点）："
<< std::endl;
for (size_t i = 0; i < cloud->points.size(); ++i)
std::cerr << i << " " << cloud->points[i].x
<< " \t" << cloud->points[i].y
<< "\t\t" << cloud->points[i].z
<< std::end;
[/cpp]

2.第二步：指定模型和可靠性参数

[cpp]

//创建模型参数对象，这里指的是平面模型的方程ax+by+cz+d=0的参数
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
//创建内点集合，用于存储分割得到的内点结果
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
// 创建分割对象
pcl::SACSegmentation&amp;amp;amp;amp;lt;pcl::PointXYZ&amp;amp;amp;amp;gt; seg;
// 设置模型系数需要优化
seg.setOptimizeCoefficients(true);
// 必须配置
seg.setModelType(pcl::SACMODEL_PLANE);
//指定当前模型是平面模型
seg.setMethodType(pcl::SAC_RANSAC);
//指定分割算法采用随机采样一致性算法
seg.setDistanceThreshold(0.01);//设置距离阈值

[/cpp]

 

 

3.第三步：开始分割

[cpp]
seg.setInputCloud(cloud);//输入点云
seg.segment(*inliers, *coefficients);//分割点云
[/cpp]

4.第四步：查看输出的模型系数和内点

[java]
//如果内点个数为0，则分割失败
if (inliers->indices.size() == 0) {
PCL_ERROR("对于给定数据集，无法估计出一个平面模型");
return (-1);
}
std::cerr << "分割完毕，模型参数： " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "模型内点个数: "
<< 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
<< " \t" << cloud->points[inliers->indices[i]].y
<< "\t\t" << cloud->points[inliers->indices[i]].z
<< std::endl; return (0);
}

[/java]

PCL库源码摘要

计算模型系数abcd

[cpp]

pcl::SampleConsensusModelPlane<PointT>::computeModelCoefficients(
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) {
// Compute the plane coefficients from the 3 given points in a straightforward manner
// calculate the plane normal
n = (p2 – p1) x(p3 – p1) = cross(p2 – p1, p3 – p1)model_coefficients.resize(4);
model_coefficients[0] = p1p0[1] * p2p0[2] – p1p0[2] * p2p0[1];
model_coefficients[1] = p1p0[2] * p2p0[0] – p1p0[0] * p2p0[2];
model_coefficients[2] = p1p0[0] * p2p0[1] – p1p0[1] * p2p0[0];
model_coefficients[3] = 0;
// Normalizemodel_coefficients.normalize ();
// … + d = 0
model_coefficients[3] = -1 * (model_coefficients.template head<4>().dot(p0.matrix()));
}

[/cpp]

 

计算点到平面的距离

[cpp]

template <typename PointT> voidpcl::SampleConsensusModelPlane<PointT>::getDistancesToModel
(
const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
Eigen::Vector4f pt(input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
1);
distances[i] = fabs(model_coefficients.dot(pt));
}

[/cpp]

计算内点个数

[cpp]

for (size_t i = 0; i < indices_->size(); ++i) {
// Calculate the distance from the point to the plane normal as the dot product
// D = (P-A).N/|N|
Eigen::Vector4f pt(input_->points[(*indices_)[i]].x,
input_->points[(*indices_)[i]].y,
input_->points[(*indices_)[i]].z,
1);
if (fabs(model_coefficients.dot(pt)) < threshold)
nr_p++;
}

[/cpp]