PCL随机采样一致性：RANSAC 圆拟合（二维圆 + 空间圆）

Posted 2021-08-11 借我十斤肉

文章目录

• • 1 二维圆
  • 2 空间圆

1 二维圆

输入3个点，返回3个模型系数

3个模型系数

coefficient	含义
coefficient[0]	二维圆圆心的 x 坐标
coefficient[1]	二维圆圆心的 y 坐标
coefficient[2]	二维圆半径

pcl三点定圆源码（xoy平面）：

template <typename PointT> bool
pcl::SampleConsensusModelCircle2D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 3 samples
  if (samples.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::computeModelCoefficients] Invalid set of samples given (%lu)!\\n", samples.size ());
    return (false);
  }

  model_coefficients.resize (3);

  Eigen::Vector2d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y);
  Eigen::Vector2d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y);
  Eigen::Vector2d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y);

  Eigen::Vector2d u = (p0 + p1) / 2.0;
  Eigen::Vector2d v = (p1 + p2) / 2.0;

  Eigen::Vector2d p1p0dif = p1 - p0;
  Eigen::Vector2d p2p1dif = p2 - p1;
  Eigen::Vector2d uvdif   = u - v;

  Eigen::Vector2d m (- p1p0dif[0] / p1p0dif[1], - p2p1dif[0] / p2p1dif[1]);

  // Center (x, y)
  model_coefficients[0] = static_cast<float> ((m[0] * u[0] -  m[1] * v[0]  - uvdif[1] )             / (m[0] - m[1]));
  model_coefficients[1] = static_cast<float> ((m[0] * m[1] * uvdif[0] +  m[0] * v[1] - m[1] * u[1]) / (m[0] - m[1]));

  // Radius
  model_coefficients[2] = static_cast<float> (sqrt ((model_coefficients[0] - p0[0]) * (model_coefficients[0] - p0[0]) +
                                                    (model_coefficients[1] - p0[1]) * (model_coefficients[1] - p0[1])));
  return (true);
}

实现代码：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_circle.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;

int main()
{
	//---------------------------------------------
	//-------------------加载点云-------------------
	//---------------------------------------------
	cout << "->正在加载点云..." << endl;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile("circle2D_gauss_noise.pcd", *cloud) < 0)
	{
		PCL_ERROR("警告！点云文件不存在！\\a\\n");
		system("pause");
		return -1;
	}

	//---------------------------------------------
	//-------------------模型估计-------------------
	//---------------------------------------------
	cout << "->正在估计二维圆..." << endl;
	pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr model_circle2D(new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>(cloud));	//选择拟合点云与几何模型
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_circle2D);	//创建随机采样一致性对象
	ransac.setDistanceThreshold(0.01);	//设置距离阈值，与模型距离小于0.01的点作为内点
	ransac.setMaxIterations(10000);		//设置最大迭代次数
	ransac.computeModel();				//执行模型估计

	vector<int> inliers;				//存储内点索引的向量
	ransac.getInliers(inliers);			//提取内点对应的索引
	
	/// 根据索引提取内点
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_circle(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_circle);

	/// 输出模型参数(x-x0)^2 + (y-y0)^2 = r^2;
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);
	cout << "二维圆方程为：\\n"
		<<"(x - "<< coefficient[0] << ")^2 + "
		<<"(y - "<< coefficient[1] << ")^2 = "
		<< coefficient[2] <<"^2"
		<< endl;

	//==========================================↓ 可视化拟合结果 ↓==========================================

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));

	viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");													//添加原始点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");	//点的大小

	viewer->addPointCloud<pcl::PointXYZ>(cloud_circle, "circle");											//添加拟合点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "circle");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "circle");	//点的大小

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	//==========================================↑ 可视化拟合结果 ↑==========================================

	return 0;
}

输出结果：

->正在加载点云...
->正在估计二维圆...
二维圆方程为：
(x - 1)^2 + (y - 2)^2 = 4^2

可视化结果：

2 空间圆

输入3个点，返回7个模型系数

7个模型系数

coefficient	含义
coefficient[0]	空间圆圆心的 x 坐标
coefficient[1]	空间圆圆心的 y 坐标
coefficient[2]	空间圆圆心的 z 坐标
coefficient[3]	空间圆半径
coefficient[4]	空间圆法向量的 x 分量
coefficient[5]	空间圆法向量的 y 分量
coefficient[6]	空间圆法向量的 z 分量

pcl空间三点定圆源码：

template <typename PointT> bool
pcl::SampleConsensusModelCircle3D<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
  // Need 3 samples
  if (samples.size () != 3)
  {
    PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\\n", samples.size ());
    return (false);
  }

  model_coefficients.resize (7);   //needing 7 coefficients: centerX, centerY, centerZ, radius, normalX, normalY, normalZ

  Eigen::Vector3d p0 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z);
  Eigen::Vector3d p1 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z);
  Eigen::Vector3d p2 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z);


  Eigen::Vector3d helper_vec01 = p0 - p1;
  Eigen::Vector3d helper_vec02 = p0 - p2;
  Eigen::Vector3d helper_vec10 = p1 - p0;
  Eigen::Vector3d helper_vec12 = p1 - p2;
  Eigen::Vector3d helper_vec20 = p2 - p0;
  Eigen::Vector3d helper_vec21 = p2 - p1;

  Eigen::Vector3d common_helper_vec = helper_vec01.cross (helper_vec12);

  double commonDividend = 2.0 * common_helper_vec.squaredNorm ();

  double alpha = (helper_vec12.squaredNorm () * helper_vec01.dot (helper_vec02)) / commonDividend;
  double beta =  (helper_vec02.squaredNorm () * helper_vec10.dot (helper_vec12)) / commonDividend;
  double gamma = (helper_vec01.squaredNorm () * helper_vec20.dot (helper_vec21)) / commonDividend;

  Eigen::Vector3d circle_center = alpha * p0 + beta * p1 + gamma * p2;

  Eigen::Vector3d circle_radiusVector = circle_center - p0;
  double circle_radius = circle_radiusVector.norm ();
  Eigen::Vector3d circle_normal = common_helper_vec.normalized (); 

  model_coefficients[0] = static_cast<float> (circle_center[0]);
  model_coefficients[1] = static_cast<float> (circle_center[1]);
  model_coefficients[2] = static_cast<float> (circle_center[2]);
  model_coefficients[3] = static_cast<float> (circle_radius);
  model_coefficients[4] = static_cast<float> (circle_normal[0]);
  model_coefficients[5] = static_cast<float> (circle_normal[1]);
  model_coefficients[6] = static_cast<float> (circle_normal[2]);
   
 return (true);
}

实现代码

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_circle3D.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std;

int main()
{
	//---------------------------------------------
	//-------------------加载点云-------------------
	//---------------------------------------------
	cout << "->正在加载点云..." << endl;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile("circle2D_gauss_noise.pcd", *cloud) < 0)
	{
		PCL_ERROR("警告！点云文件不存在！\\a\\n");
		system("pause");
		return -1;
	}

	//---------------------------------------------
	//-------------------模型估计-------------------
	//---------------------------------------------
	cout << "->正在估计空间圆..." << endl;
	pcl::SampleConsensusModelCircle3D<pcl::PointXYZ>::Ptr model_circle3D(new pcl::SampleConsensusModelCircle3D<pcl::PointXYZ>(cloud));	//选择拟合点云与几何模型
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_circle3D);	//创建随机采样一致性对象
	ransac.setDistanceThreshold(0.01);	//设置距离阈值，与模型距离小于0.01的点作为内点
	ransac.setMaxIterations(10000);		//设置最大迭代次数
	ransac.computeModel();				//执行模型估计
	vector<int> inliers;				//存储内点索引的向量
	ransac.getInliers(inliers);			//提取内点对应的索引

	/// 根据索引提取内点
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_circle(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *cloud_circle);

	/// 输出模型参数(x-x0)^2 + (y-y0)^2 = r^2;
	Eigen::VectorXf coefficient;
	ransac.getModelCoefficients(coefficient);

	cout << "空间圆心："
		<< "(" << coefficient[0] << ","
		<< coefficient[1] << ","
		<< coefficient[2] << ")"
		<< endl;
	cout << "半径：" << coefficient[3] << endl;
	cout << "空间圆法向量："
		<< "(" << coefficient[4] << ","
		<< coefficient[5] << ","
		<< coefficient[6] << ")"
		<< endl;


	//==========================================↓ 可视化拟合结果 ↓==========================================

	pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("拟合结果"));

	viewer->addPointCloud<pcl::PointXYZ>(cloud, "cloud");													//添加原始点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud");	//点的大小

	viewer->addPointCloud<pcl::PointXYZ>(cloud_circle, "circle");											//添加模型点云
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "circle");	//颜色
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "circle");	//点的大小

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(以上是关于PCL随机采样一致性：RANSAC 圆拟合（二维圆 + 空间圆）的主要内容，如果未能解决你的问题，请参考以下文章
 
 PCL随机采样一致性：RANSAC 平面拟合
 PCL：RANSAC圆柱体拟合（详细注释，对新手友好！）
 点云处理技术之PCL随机采样一致算法（Random sample consensus，RANSAC）
 PCL 平面拟合——RANSAC
 PCL：RANSAC 空间直线拟合
 数据拟合：最小二乘二维圆拟合的C++实现（另一种方法）