三维空间点云拟合问题

---

前言

此代码为三维空间内点云数据拟合圆的计算过程，主要求解拟合后的圆心、半径及圆法向量，点云数据来源于激光跟踪仪的采集。可应用于工业机器人单关节运行速度测试、结构杆长测试等。

---

代码

clear;clc;close all;
[FileNmae,PathName]=uigetfile({'*.*'});
str=[PathName,FileNmae];
D=importdata(str);
M=D.data; 
[A,C,R]=CircleFit(M); 
 
figure(1)
subplot(2,1,1);
h1=plot3(M(:,1),M(:,2),M(:,3),'*'); 
%%%%   绘制空间圆  %%%%
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');
hold on
h3=plot3(c(1),c(2),c(3),'.');
xlabel('x轴');
ylabel('y轴');
zlabel('z轴');
legend([h1 h2 h3],'控制点','拟合圆','圆心');
grid on

%% 
function [A,C,R]=CircleFit(M)
[row,~]=size(M); 
L1=ones(row,1);
A=(M'*M)\M'*L1;       % 最小二乘法求解平面法向量
%n个点两点向量，共有n(n-1)/2种
count=0; 
for j=1:row-1
    for i=j+1:row
        count=count+1;
        M2(count,:)=M(j,:)-M(i,:);
        L2(count,1)=(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
B=zeros(4,3);
B(1:3,1:3)=(M2'*M2);
B(4,1:3)=A';
B(1:3,4)=A;
D=[M2'*L2;1]; 
C0=B'\D;   % 求解空间圆圆心坐标
C=C0(1:3); 

for i=1:row
    radius(i,1)=sqrt((M(i,1)-C(1))^2+(M(i,2)-C(2))^2+(M(i,3)-C(3))^2);
    d(i,1)=abs(A(1)*M(i,1)+A(2)*M(i,2)+A(3)*M(i,3)-1)/sqrt(A(1)^2+A(2)^2+A(3)^2);
end
R=mean(radius) ; %  空间圆拟合半径
subplot(2,1,2);
rate=radius-R;
plot(rate,'.-');
hold on
plot(d);
xlabel('样点数');ylabel('误差');
legend('圆柱度误差','平面度误差');
grid on
end

---

运算结果

样点数据

P1 -3.314982 -152.666170 780.558879
P2 26.871039 -151.326424 780.071988
P3 55.825499 -146.375608 779.618815
P4 108.986720 -126.960903 778.789820
P5 147.255969 -102.729131 778.218709
P6 192.210975 -56.888118 777.542537
P7 216.245051 -17.043479 777.190037
P8 233.245803 31.710441 776.979344
P9 239.173715 92.510888 776.959304
P10 225.109482 165.772094 777.263187
P11 191.800452 227.404155 777.859815
P12 140.146306 277.974087 778.748015
P13 97.785023 302.213009 779.459391
P14 74.707437 311.030769 779.843070
P15 45.928247 318.418403 780.323075