已知三维空间离散点座标(xi, yi, zi),构建一个空间圆使得空间点尽可能靠近拟合的空间圆。效果如下图
首先,所有离散点尽可能在一个平面上,平面方程可表示为
(1)
写成矩阵形式为,
,式中
,, (2)
这是一个超定方程求解,根据最小二乘法,可以求出,即平面的法向向量。
假设所有离散点都在圆上,那么任意两点的连线的中垂线必过圆心。设圆心C(x0,y0,z0),取两个点P1(x1,y1,z1)与P2(x2,y2,z2),则P1和P2连线的向量vector1表示为(x2-x1,y2-y1,z2-z1),P1和P2连线的中点座标P12为
圆心C与P12连线向量vector2为。要想P1与P2在圆上,则满足vector1*vecotor2=0,即
整理一下,有
,
式中 ,
所有点都在圆上,则有
(3)
写成矩阵形式 ,
式中,,
上述方程亦为超定方程,变换成适定形式为
(4)
由于圆心C必在前述控制的平面内,因此满足, 即
(5)
A为平面的法向量,通过 已求出,因而可将式(4)和式(5)合并,写成一个扩展式进行求解
,式中
,
则求解圆心座标
圆的半径可由所有点到圆心的距离的平均值确定:
具体matlab程序实现如下
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Project: 3D circle fitting
% Author: jiangjp
% [email protected]
% Wuhan University of Technology
% Date: 25/10/2019
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear all;
clc;
close all;
%%% 无误差空间圆拟合点 %%%
% M=[-0.3 0.46 0.83;...
% -0.2 0.254 0.946;...
% -0.1 0.111 0.989;...
% 0 0 1;...
% 0.1 -0.0910 0.991;...
% 0.2 -0.166 0.966;...
% 0.3 -0.227 0.927;...
% 0.4 -0.275 0.874;...
% 0.5 -0.309 0.809;...
% 0.6 -0.329 0.729;...
% 0.7 -0.332 0.632;...
% 0.8 -0.312 0.512;...
% 0.9 -0.254 0.354;...
% 0.8 0.512 -0.312;...
% 0.7 0.632 -0.332;...
% 0.6 0.729 -0.329;...
% 0.5 0.809 -0.309;...
% 0.4 0.874 -0.274;...
% 0.3 0.927 -0.227;...
% 0.2 0.966 -0.166;...
% 0.1 0.991 -0.091;...
% 0 1 0;...
% -0.1 0.989 0.111;...
% -0.2 0.946 0.254;...
% -0.3 0.83 0.45];
%%% 含误差空间圆拟合点 %%%
M=[11.5713 6.9764 10.4685;...
11.5859 9.088 13.5831;...
11.5802 11.1949 14.6103;...
11.5542 13.312 14.7279;...
11.5692 15.3806 14.0576;...
11.5632 17.4873 12.1397;...
11.5598 17.8894 6.4025;...
11.5577 15.8714 4.0703;...
11.5729 13.8578 3.232;...
11.5657 11.8711 3.1326;...
11.5706 9.8797 3.7866;...
11.5663 7.8676 5.5348];
[num dim]=size(M);
L1=ones(num,1);
A=inv(M'*M)*M'*L1; % 求解平面法向量
B=zeros(num-1,3);
for i=1:num-1
B(i,:)=M(i+1,:)-M(i,:);
end
L2=zeros(num-1,1);
for i=1:num-1
L2(i)=(M(i+1,1)^2+M(i+1,2)^2+M(i+1,3)^2-M(i,1)^2-M(i,2)^2-M(i,3)^2)/2;
end
D=[B'*B;A'] % 将平面作为限制条件加入方程组
L3=[B'*L2;1]
C=inv(D'*D)*D'*L3 % 求解空间圆圆心座标
radius=0;
for i=1:num
tmp=M(i,:)-C';
radius=radius+sqrt(tmp(1)^2+tmp(2)^2+tmp(3)^2);
end
r=radius/num % 空间圆拟合半径
figure
h1=plot3(M(:,1),M(:,2),M(:,3),'*');
set(gca,'xlim',[11.4 11.7]);
%%%% 绘制空间圆 %%%%
n=A;
c=C;
theta=(0:2*pi/100:2*pi)'; % theta角从0到2*pi
a=cross(n,[1 0 0]); % n与i叉乘,求取a向量
if ~any(a) % 如果a为零向量,将n与j叉乘
a=cross(n,[0 1 0]);
end
b=cross(n,a); % 求取b向量
a=a/norm(a); % 单位化a向量
b=b/norm(b); % 单位化b向量
c1=c(1)*ones(size(theta,1),1);
c2=c(2)*ones(size(theta,1),1);
c3=c(3)*ones(size(theta,1),1);
x=c1+r*a(1)*cos(theta)+r*b(1)*sin(theta); % 圆上各点的x座标
y=c2+r*a(2)*cos(theta)+r*b(2)*sin(theta); % 圆上各点的y座标
z=c3+r*a(3)*cos(theta)+r*b(3)*sin(theta); % 圆上各点的z座标
hold on;
h2=plot3(x,y,z,'-r');
xlabel('x轴')
ylabel('y轴')
zlabel('z轴')
legend([h1 h2],'控制点','拟合圆');
grid on
程序运行结果
1. 控制点无误差空间圆拟合,2 控制点含误差空间圆拟合
参考文献:
[1] 李英硕,杨帆,袁兆奎. 空间圆形拟合检测新方法. 测绘科学. 2013, 38(6): 147-148.