最小二乘法拟合三维空间中的圆及其C++实现

概要

这篇文章描述了使用最小二乘法拟合三维空间中的圆的理论方法及其C++实现代码

理论

已知

我们已知的是空间中圆的一系列采样点

$p=\left[ \begin{matrix} x & y & z \\ \end{matrix} \right]$

其中x、y、z均为n*1的列向量

拟合圆所在平面

平面方程可以表述如下

$Ax+By+Cz+1=0$

将公式进行变换之后可以得到

$\left[ \begin{matrix} x & y & z \\ \end{matrix} \right]\left[ \begin{matrix} A \\ B \\ C \\ \end{matrix} \right]=\left[ -1 \right]$

即得到了一个线性表达式，再使用最小二乘法计算参数A、B、C即可

考虑到系数矩阵可能奇异，可以采用奇异值分解法（SVD）来算，这里就不详细展开了

得到的矢量即为平面的法矢

$n=\left[ \begin{matrix} A \\ B \\ C \\ \end{matrix} \right]$

旋转采样点

这一步的目的是使得采样点所在平面平行于XoY平面，方便进行下一步的圆拟合

这里使用的是通过转轴和转角来构造旋转矩阵

法矢n的第三项是法矢和Z轴的夹角余弦值，对应的夹角可通过反三角函数算出

$\theta ={{\cos }^{-1}}\left( C \right)$

然后所需的转轴可以通过法矢叉乘Z轴得到

$v=\operatorname{cross}\left( \left[ \begin{matrix} A \\ B \\ C \\ \end{matrix} \right],\left[ \begin{matrix} 0 \\ 0 \\ 1 \\ \end{matrix} \right] \right)$

构造旋转矩阵的公式如下

$R\left( v,\theta \right)=\left[ \begin{matrix} {{v}_{x}}{{v}_{x}}\left( 1-\cos \theta \right)+\cos \theta & {{v}_{y}}{{v}_{x}}\left( 1-\cos \theta \right)-{{v}_{z}}\sin \theta & {{v}_{z}}{{v}_{x}}\left( 1-\cos \theta \right)+{{v}_{y}}\sin \theta \\ {{v}_{x}}{{v}_{y}}\left( 1-\cos \theta \right)+{{v}_{z}}\sin \theta & {{v}_{y}}{{v}_{y}}\left( 1-\cos \theta \right)+\cos \theta & {{v}_{z}}{{v}_{y}}\left( 1-\cos \theta \right)-{{v}_{x}}\sin \theta \\ {{v}_{x}}{{v}_{z}}\left( 1-\cos \theta \right)-{{v}_{y}}\sin \theta & {{v}_{y}}{{v}_{z}}\left( 1-\cos \theta \right)+{{v}_{x}}\sin \theta & {{v}_{z}}{{v}_{z}}\left( 1-\cos \theta \right)+\cos \theta \\ \end{matrix} \right]$

对采样点进行旋转

${{p}_{rotate}}={{\left( R\left( v,\theta \right)\times {{p}^{T}} \right)}^{T}}$

拟合圆心

平面圆的公式为

${{\left( x-D \right)}^{2}}+{{\left( y-E \right)}^{2}}=F$

展开并移项可以获得

$\left[ \begin{matrix} 2x & 2y & -1 \\ \end{matrix} \right]\left[ \begin{matrix} D \\ E \\ {{D}^{2}}+{{E}^{2}}-F \\ \end{matrix} \right]={{x}^{2}}+{{y}^{2}}$

再次获得一个线性方程，将 ${{p}_{rotate}}$ 代入，再利用SVD法，即可求得圆心

${{center}_{rotate}}=\left[ \begin{matrix} D \\ E \\ mean\left( {{p}_{rotate}}\left( 3 \right) \right) \\ \end{matrix} \right]$

如有需要，圆半径可以通过拟合出来的第三项得到

将拟合的圆心旋转回去

实际的圆心坐标如下

$center=R{{\left( v,\theta \right)}^{-1}}\times cente{{r}_{rotate}}=R{{\left( v,\theta \right)}^{T}}\times cente{{r}_{rotate}}$

C++实现

这里使用了第三方库Eigen来进行奇异值分解和旋转变换

#include <iostream>
#include <Eigen/Dense>
#define _USE_MATH_DEFINES
#include <math.h>
int main()
{
	// init objects
	Eigen::Matrix<double, 1000, 1> theta;
	Eigen::Matrix<double, 1000, 3> p_xoy;
	double r = 0;
	double noise_amplitude = 0;
	Eigen::Isometry3d t_circle_world;
	Eigen::Matrix<double, 1000, 3> p;
	Eigen::MatrixXd p_plane(1000, 3);
	Eigen::MatrixXd b_plane(1000, 1);
	Eigen::MatrixXd x_plane(3, 1);
	Eigen::Vector3d plane_rotate;
	double theta_rotate = 0;
	Eigen::Vector3d vector_rotate;
	Eigen::Isometry3d r_rotate;
	Eigen::MatrixXd p_rotate(1000, 3);
	Eigen::MatrixXd a_circle(1000, 3);
	Eigen::MatrixXd x_circle(3, 1);
	Eigen::MatrixXd b_circle(1000, 1);
	Eigen::Vector3d center_circle;
	Eigen::Isometry3d r_back;
	Eigen::Vector3d center_back;
	// create points on a circle on XoY plane
	theta = Eigen::Matrix<double, 1000, 1>::LinSpaced(0, 2 * M_PI);
	r = 5;
	p_xoy.array().col(0) = r * Eigen::cos(theta.array());
	p_xoy.array().col(1) = r * Eigen::sin(theta.array());
	p_xoy.col(2).setZero();
	std::cout << "p_xoy:" << std::endl;
	std::cout << p_xoy << std::endl;
	// add noise
	noise_amplitude = 1;
	p_xoy += noise_amplitude * Eigen::Matrix<double, 1000, 3>::Random();
	// translate and rotate the points
	t_circle_world = Eigen::Translation3d(1, 2, 3)
		* Eigen::AngleAxisd(M_PI / 3, Eigen::Vector3d::UnitZ())
		* Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY())
		* Eigen::AngleAxisd(M_PI / 5, Eigen::Vector3d::UnitX());
	p = (t_circle_world * p_xoy.transpose()).transpose();
	std::cout << "t_circle_world: " << std::endl;
	std::cout << t_circle_world.matrix() << std::endl;
	// fit plane
	p_plane = p;
	b_plane = -Eigen::MatrixXd::Ones(1000, 1);
	x_plane = p_plane.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_plane).normalized(); // normalized is important!
	std::cout << "x_plane: " << std::endl;
	std::cout << x_plane << std::endl;
	// rotate points and project points to XoY plane
	plane_rotate = x_plane;
	theta_rotate = acos(plane_rotate(2));
	vector_rotate = plane_rotate.cross(Eigen::Vector3d::UnitZ()).normalized(); // normalized is important!
	r_rotate = Eigen::AngleAxisd(theta_rotate, vector_rotate);
	p_rotate = (r_rotate * p.transpose()).transpose();
	std::cout << "p_rotate: " << std::endl;
	std::cout << p_rotate << std::endl;
	// fit circle center
	a_circle.col(0) = 2 * p_rotate.col(0);
	a_circle.col(1) = 2 * p_rotate.col(1);
	a_circle.col(2) = -Eigen::MatrixXd::Ones(1000, 1);
	b_circle = p_rotate.col(0).array().pow(2) + p_rotate.col(1).array().pow(2);
	x_circle = a_circle.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_circle);
	center_circle = x_circle;
	center_circle(2) = p_rotate.col(2).mean();
	std::cout << "center_circle:" << std::endl;
	std::cout << center_circle << std::endl;
	// rotate the fitted circle center back
	r_back = r_rotate.inverse();
	center_back = r_back * center_circle;
	std::cout << "center_back:" << std::endl;
	std::cout << center_back << std::endl;
	return 0;
}

微小的想法

感觉这个莫名像主成分分析~

参考链接

https://zhuanlan.zhihu.com/p/97036816

https://blog.csdn.net/weixin_42946636/article/details/104873413

概要

理论

已知