空间三维直线拟合matlab代码

注意：本文是matlab代码
%% 拟合空间直线
%% 输入数据
lineData=[2222.015, 477.055, -423.390
1488.935, 470.907, -500.077
1479.015, 470.803, -501.172
1608.675, 471.967,-487.638
1747.542, 473.029, -473.021
1754.288, 473.099, -472.336
1895.950, 474.346, -457.462
1906.257, 474.470, -456.470
1674.206, 472.473, -480.760
1366.207, 469.845, -512.952
1261.768, 468.974, -523.886
];  %数据点坐标
%% 画散点图
x=lineData(:,1);
y=lineData(:,2);
z=lineData(:,3);
scatter3(x, y, z,'filled')  %散点图函数，'filled'表示画实心点
hold on;    %画了一幅图，再画另一幅图时，原来的图还在，与新图共存
%% 计算平均值（拟合的直线必过所有坐标的算数平均值）
xyz0(1)=mean(x);
xyz0(2)=mean(y);
xyz0(3)=mean(z);%拟合点坐标
%% 奇异值分解计算方向向量(第一种方法)
% 协方差矩阵奇异变换
% 所得直线的方向实际上与最大奇异值对应的奇异向量相同
% centeredLine=bsxfun(@minus,lineData,xyz0);
% [U,S,V]=svd(centeredLine);
% direction=V(:,1);%方向向量

%% 最小二乘计算方向向量(第二种方法)
A=0;
B=0;
C=0;
D=0;
E=0;
F=0;
for i=1:11
A=A+x(i)*z(i);
B=B+x(i);
C=C+y(i)*z(i);
D=D+y(i);
E=E+z(i)*z(i);
F=F+z(i);
end
Tt=[A,B;C,D]*[E,F;F,11]^-1;
R=sqrt(Tt(1)^2+Tt(2)^2+1);
direction(1)=Tt(1)/R;
direction(2)=Tt(2)/R;
direction(3)=1/R;
direction=direction';%方向向量
%% 画图
t=-100:0.1:100;
xx=xyz0(1)+direction(1)*t;
yy=xyz0(2)+direction(2)*t;
zz=xyz0(3)+direction(3)*t;
plot3(xx,yy,zz)

%% 计算点到直线的距离
for i=1:11
T(i)=(-direction(1)*xyz0(1)+direction(1)*lineData(i,1)-direction(2)*xyz0(2)+direction(2)*lineData(i,2)-direction(3)*xyz0(3)+direction(3)*lineData(i,3))/(direction(1)^2+direction(2)^2+direction(3)^2);
xx1=xyz0(1)+direction(1)*T(i);
yy1=xyz0(2)+direction(2)*T(i);
zz1=xyz0(3)+direction(3)*T(i);
D(i)=sqrt((xx1-lineData(i,1))^2+(yy1-lineData(i,2))^2+(zz1-lineData(i,3))^2);
end