数据拟合：最小二乘二维圆拟合的C++实现（另一种方法）

Posted 2021-11-24 没事就要敲代码

文章目录

• • 1 介绍
  • 2 代码实现
  • 3 相关链接

1 介绍

在 数据拟合（二）：最小二乘二维圆拟合的C++实现 一文中已经详细介绍了最小二乘二维圆拟合的原理与实现，这里给出另一种实现方法，最终，两种方法对同一数据拟合，结果一致。

2 代码实现

老规矩，代码分三部分构成

• main.cpp
• least_square_circle2D_fitting.h
• least_square_circle2D_fitting.cpp

main.cpp

#include "least_square_circle2D_fitting.h"

int main()
{
	//----------------------- 加载点云 -----------------------
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	if (pcl::io::loadPCDFile("circle2D.pcd", *cloud) < 0)
	{
		PCL_ERROR("\\a->点云文件不存在！\\n");
		system("pause");
		return -1;
	}
	cout << "->加载点云的点数：" << cloud->points.size() << endl;
	//=======================================================

	//------------------ 最小二乘二维圆拟合 -------------------
	LeastSquareCircleFittingSolver lscf;
	lscf.setInputCloud(cloud);
	lscf.setOrientation("xoy");
	lscf.setMinPointsNum(9);
	lscf.leastSquaresCircle();
	vector<double> centerRadius;
	centerRadius = lscf.getCenterRadius();
	cout << "圆心：(" << centerRadius[0] << "," << centerRadius[1] << ")" << endl
		 << "半径：" << centerRadius[2] << endl;
	//=======================================================

	return 0;
}

输出结果：

->加载点云的点数：63
圆心：(1,2)
半径：4

least_square_circle2D_fitting.h

#pragma once
#include <pcl/io/pcd_io.h>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

using namespace std;

class LeastSquareCircleFittingSolver
{
public:
	/**
	* @brief   ：设置输入点云
	* @param[I]：cloud_in（输入点云）
	* @param[O]：none
	* @return  ：none
	* @note	   ：
	**/
	void setInputCloud(PointCloudT::Ptr cloud_in);

	/**
	* @brief   ：设置二维圆所在的坐标平面
	* @param[I]：orientation（xoy | yoz | xoz）
	* @param[O]：none
	* @return  ：none
	* @note	   ：
	**/
	void setOrientation(string orientation);

	/**
	* @brief   ：设置拟合所需的最小点数
	* @param[I]：PtNum（拟合所需的最小点数，PtNum>=3）
	* @param[O]：none
	* @return  ：none
	* @note	   ：
	**/
	void setMinPointsNum(int PtNum);

	/**
	* @brief   ：执行最小二乘二维圆拟合
	* @param[I]：none
	* @param[O]：none
	* @return  ：none
	* @note	   ：
	**/
	void leastSquaresCircle();

	/**
	* @brief   ：获取拟合圆心和半径
	* @param[I]：none
	* @param[O]：none
	* @return  ：vector<double>（x,y,r）
	* @note	   ：
	**/
	vector<double> getCenterRadius();

private:

	PointCloudT::Ptr m_cloud_in;	//输入点云
	bool is_setInputCloud = false;

	string m_orientation;			//二维圆所在的坐标平面
	bool is_setOrientation = false;

	int m_minPtNum;					//拟合所需的最小点数
	bool is_setMinPointsNum = false;

	vector<double> m_centerRadius;	//存放圆心和半径

};

least_square_circle2D_fitting.cpp

#include "least_square_circle2D_fitting.h"

/**
* @brief   ：设置输入点云
* @param[I]：cloud_in（输入点云）
* @param[O]：none
* @return  ：none
* @note	   ：
**/
void LeastSquareCircleFittingSolver::setInputCloud(PointCloudT::Ptr cloud_in)
{
	m_cloud_in = cloud_in;
	is_setInputCloud = true;
}

/**
* @brief   ：设置二维圆所在的坐标平面
* @param[I]：orientation（xoy | yoz | xoz）
* @param[O]：none
* @return  ：none
* @note	   ：
**/
void LeastSquareCircleFittingSolver::setOrientation(string orientation)
{
	m_orientation = orientation;
	is_setOrientation = true;
}

/**
* @brief   ：设置拟合所需的最小点数
* @param[I]：PtNum（拟合所需的最小点数，PtNum>=3）
* @param[O]：none
* @return  ：none
* @note	   ：
**/
void LeastSquareCircleFittingSolver::setMinPointsNum(int PtNum)
{
	if (PtNum > 2)
	{
		m_minPtNum = PtNum;
		is_setMinPointsNum = true;
	}
	else
	{
		PCL_ERROR("\\a->二维圆拟合至少需要三个点！\\n");
	}
}

/**
* @brief   ：执行最小二乘二维圆拟合
* @param[I]：none
* @param[O]：none
* @return  ：none
* @note	   ：
**/
void LeastSquareCircleFittingSolver::leastSquaresCircle()
{
	int length = (int)m_cloud_in->size();	//数据点个数

	Eigen::MatrixXd A(length, 3);		//length×3阶矩阵，每一行为一个点，第三列数据均为1
	Eigen::VectorXd AFirst(length);		//第一列数据，iv
	Eigen::VectorXd ASec(length);		//第二列数据，dv
	Eigen::VectorXd AFirstSquared(length);//列向量，存放iv的平方
	Eigen::VectorXd ASecSquared(length);//列向量，存放dv的平方
	Eigen::VectorXd ASquaredRes(length);
	Eigen::VectorXd b(length);	//b = iv^2 + dv^2 
	Eigen::VectorXd c(3);		//SVD分解结果

	if (length > m_minPtNum)
	{
		if (m_orientation == "yoz")
		{
			for (int i = 0; i < length; i++)
			{
				A(i, 0) = m_cloud_in->points[i].y;
				A(i, 1) = m_cloud_in->points[i].z;
				A(i, 2) = 1;		//恒为1
			}
		}
		else if (m_orientation == "xoz")
		{
			for (int i = 0; i < length; i++)
			{
				A(i, 0) = m_cloud_in->points[i].x;
				A(i, 1) = m_cloud_in->points[i].z;
				A(i, 2) = 1;		//恒为1
			}
		}
		else if (m_orientation == "xoy")
		{
			for (int i = 0; i < length; i++)
			{
				A(i, 0) = m_cloud_in->points[i].x;
				A(i, 1) = m_cloud_in->points[i].y;
				A(i, 2) = 1;		//恒为1
			}
		}
		else
		{
			PCL_ERROR("\\a->请按要求输入！（xoy|xoz|yoz）\\n");
			system("pause");
			abort();
		}

		for (int i = 0; i < length; i++)
		{
			AFirst(i) = A(i, 0);
			ASec(i) = A(i, 1);
		}

		for (int i = 0; i < length; i++)
		{
			AFirstSquared(i) = AFirst(i) * AFirst(i);
			ASecSquared(i) = ASec(i) * ASec(i);
		}

		b = AFirstSquared + ASecSquared;
		c = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);	//SVD分解，并将结果赋给c

		//圆心与半径提取
		double c1, c2, radius;
		c1 = c(0) * 0.5;
		c2 = c(1) * 0.5;
		radius = sqrt((c(0) * c(0) + c(1) * c(1)) / 4 + c(2));

		m_centerRadius.push_back(c1);
		m_centerRadius.push_back(c2);
		m_centerRadius.push_back(radius);
	}
	else
	{
		PCL_ERROR("未成功拟合圆！\\a\\n");
	}
}

/**
* @brief   ：获取拟合圆心和半径
* @param[I]：none
* @param[O]：none
* @return  ：vector<double>（x,y,r）
* @note	   ：
**/
vector<double> LeastSquareCircleFittingSolver::getCenterRadius()
{
	return m_centerRadius;
}

3 相关链接

点云数据处理常用算法 ❤️❤️❤️ 目录

PCL点云数据处理基础❤️❤️❤️目录