pcl画圆球_PCL——（10）PCL中使用随机采样一致性模型

@

一、RANSAC随机采样一致性算法的介绍

RANSAC是“RANdom SAmple Consensus(随机抽样一致)”的缩写。它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。

数 据分两种：有效数据(inliers)和无效数据(outliers)。偏差不大的数据称为有效数据，偏差大的数据是无效数据。如果有效数据占大多数，无 效数据只是少量时，我们可以通过最小二乘法或类似的方法来确定模型的参数和误差；如果无效数据很多(比如超过了50%的数据都是无效数据)，最小二乘法就 失效了，我们需要新的算法

一个简单的例子是从一组观测数据中找出合适的2维直线。假设观测数据中包含局内点和局外点，其中局内点近似的被直线所通过，而局外点远离于直线。简单的最 小二乘法不能找到适应于局内点的直线，原因是最小二乘法尽量去适应包括局外点在内的所有点。相反，RANSAC能得出一个仅仅用局内点计算出模型，并且概 率还足够高。但是，RANSAC并不能保证结果一定正确，为了保证算法有足够高的合理概率，我们必须小心的选择算法的参数。

左图：包含很多局外点的数据集 . 右图：RANSAC找到的直线(局外点并不影响结果)

详细介绍可以查看我的另一篇博文。

二、最小中值法(LMedS)

三、PCL中使用随机采样一致性模型

#include

#include

#include

#include

#include

#include

#include

#include

#include

#include

boost::shared_ptr<:visualization::pclvisualizer>

simpleVis (pcl::PointCloud<:pointxyz>::ConstPtr cloud)

{

// --------------------------------------------

// -----Open 3D viewer and add point cloud-----

// --------------------------------------------

boost::shared_ptr<:visualization::pclvisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));

viewer->setBackgroundColor (0, 0, 0);

viewer->addPointCloud<:pointxyz> (cloud, "sample cloud");

viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");

//viewer->addCoordinateSystem (1.0, "global");

viewer->initCameraParameters ();

return (viewer);

}

/******************************************************************************************************************

对点云进行初始化，并对其中一个点云填充点云数据作为处理前的的原始点云，其中大部分点云数据是基于设定的圆球和平面模型计算

而得到的坐标值作为局内点，有1/5的点云数据是被随机放置的作为局外点。

*****************************************************************************************************************/

int

main(int argc, char** argv)

{

// 初始化点云对象

pcl::PointCloud<:pointxyz>::Ptr cloud (new pcl::PointCloud<:pointxyz>); //存储源点云

pcl::PointCloud<:pointxyz>::Ptr final (new pcl::PointCloud<:pointxyz>); //存储提取的局内点

// 填充点云数据

cloud->width = 500; //填充点云数目

cloud->height = 1; //无序点云

cloud->is_dense = false;

cloud->points.resize (cloud->width * cloud->height);

for (size_t i = 0; i < cloud->points.size (); ++i)

{

if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)

{

//根据命令行参数用x^2+y^2+Z^2=1设置一部分点云数据，此时点云组成1/4个球体作为内点

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));

}

else

{ //用x+y+z=1设置一部分点云数据，此时地拿云组成的菱形平面作为内点

cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);

cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);

if( i % 2 == 0)

cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0); //对应的局外点

else

cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);

}

}

std::vector inliers; //存储局内点集合的点的索引的向量

//创建随机采样一致性对象

pcl::SampleConsensusModelSphere<:pointxyz>::Ptr

model_s(new pcl::SampleConsensusModelSphere<:pointxyz> (cloud)); //针对球模型的对象

pcl::SampleConsensusModelPlane<:pointxyz>::Ptr

model_p (new pcl::SampleConsensusModelPlane<:pointxyz> (cloud)); //针对平面模型的对象

if(pcl::console::find_argument (argc, argv, "-f") >= 0)

{ //根据命令行参数，来随机估算对应平面模型，并存储估计的局内点

pcl::RandomSampleConsensus<:pointxyz> ransac (model_p);

ransac.setDistanceThreshold (.01); //与平面距离小于0.01 的点称为局内点考虑

ransac.computeModel(); //执行随机参数估计

ransac.getInliers(inliers); //存储估计所得的局内点

}

else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )

{

//根据命令行参数 来随机估算对应的圆球模型，存储估计的内点

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

ransac.setDistanceThreshold (.01);

ransac.computeModel();

ransac.getInliers(inliers);

}

// 复制估算模型的所有的局内点到final中

pcl::copyPointCloud<:pointxyz>(*cloud, inliers, *final);

// 创建可视化对象并加入原始点云或者所有的局内点

boost::shared_ptr<:visualization::pclvisualizer> viewer;

if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)

viewer = simpleVis(final);

else

viewer = simpleVis(cloud);

while (!viewer->wasStopped ())

{

viewer->spinOnce (100);

boost::this_thread::sleep (boost::posix_time::microseconds (100000));

}

return 0;

}

四、Sample_consensus模型支持的几何模型

SACMODEL_PLANE - used to determine plane models. The four coefficients of the plane are its Hessian Normal form: [normal_x normal_y normal_z d]

SACMODEL_LINE - used to determine line models. The six coefficients of the line are given by a point on the line and the direction of the line as: [point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z]

SACMODEL_CIRCLE2D - used to determine 2D circles in a plane. The circle's three coefficients are given by its center and radius as: [center.x center.y radius]

SACMODEL_CIRCLE3D - used to determine 3D circles in a plane. The circle's seven coefficients are given by its center, radius and normal as: [center.x, center.y, center.z, radius, normal.x, normal.y, normal.z]

SACMODEL_SPHERE - used to determine sphere models. The four coefficients of the sphere are given by its 3D center and radius as: [center.x center.y center.z radius]

SACMODEL_CYLINDER - used to determine cylinder models. The seven coefficients of the cylinder are given by a point on its axis, the axis direction, and a radius, as: [point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]

SACMODEL_CONE - used to determine cone models. The seven coefficients of the cone are given by a point of its apex, the axis direction and the opening angle, as: [apex.x, apex.y, apex.z, axis_direction.x, axis_direction.y, axis_direction.z, opening_angle]

SACMODEL_TORUS - not implemented yet

SACMODEL_PARALLEL_LINE - a model for determining a line parallel with a given axis, within a maximum specified angular deviation. The line coefficients are similar to SACMODEL_LINE .

SACMODEL_PERPENDICULAR_PLANE - a model for determining a plane perpendicular to an user-specified axis, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE .

SACMODEL_PARALLEL_LINES - not implemented yet

SACMODEL_NORMAL_PLANE - a model for determining plane models using an additional constraint: the surface normals at each inlier point has to be parallel to the surface normal of the output plane, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE .

SACMODEL_PARALLEL_PLANE - a model for determining a plane parallel to an user-specified axis, within a maximum specified angular deviation. SACMODEL_PLANE .

SACMODEL_NORMAL_PARALLEL_PLANE defines a model for 3D plane segmentation using additional surface normal constraints. The plane must lie parallel to a user-specified axis. SACMODEL_NORMAL_PARALLEL_PLANE therefore is equivalent to SACMODEL_NORMAL_PLANE + SACMODEL_PARALLEL_PLANE. The plane coefficients are similar to SACMODEL_PLANE .