三维离散点最优空间圆拟合及实现

已知三维空间离散点坐标(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)

写成矩阵形式   ​​​​​​​， 

式中，，

上述方程亦为超定方程。由于圆心C必在前述控制的平面内，因此满足​​​​​​​, 即  

                                                                                                                 (4)  

A为平面的法向量，通过 ​​​​​​​  已求出。因此可以通过构建在（4）式约束下的最优化问题来求解（3）式，即 

                                                                    （5）                 其中，为拉格朗日乘子。对关于C与求导，并令导数值为0，变换得到

                                                                                   （6）

 （注：可参考文章底部文献【1】）

则可求得圆心坐标C与，即

圆的半径可由所有点到圆心的距离的平均值确定：

具体matlab程序实现如下

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Project: 3D circle fitting
% Author: jiangjp
%         1034378054@qq.com
%         Wuhan University of Technology
% Date:   25/10/2019
% revised: 6/1/2021
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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)*num/2,3);

count=0;
for i=1:num-1
    for j=i+1:num   
        count=count+1;
        B(count,:)=M(j,:)-M(i,:);
    end    
end

L2=zeros((num-1)*num/2,1);
count=0;
for i=1:num-1
    for j=i+1:num
        count=count+1;
        L2(count)=(M(j,1)^2+M(j,2)^2+M(j,3)^2-M(i,1)^2-M(i,2)^2-M(i,3)^2)/2;
    end
end

D=zeros(4,4);
D(1:3,1:3)=(B'*B);
D(4,1:3)=A';
D(1:3,4)=A;

L3=[B'*L2;1]

C=inv(D')*(L3)   % 求解空间圆圆心坐标

C=C(1:3);

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.