matlab練習程序（空間橢圓擬合）

之前實現過三維橢圓擬合，當時是利用已知點先進行橢球擬合，再進行平面擬合，通過解兩個面的相交線得到空間橢圓函數。

如果只知道空間坐標可以用上述的方法，但是通常我們獲得空間點時會附帶時間信息，因此我們可以認為三個分量都是時間的函數，來進行擬合。

函數如下：

由於是非線性方程組，下面我們只需要用高斯牛頓法或者LM法計算非線性最小二乘就可以了。

代碼如下：

clear all;
close all;
clc;

warning off;
A = rand(1,3);
B = rand(1,3);
C = rand(1,3);

t = 0:0.01:2*pi;
p=[];
for i=1:length(t)
    x =  C(1) + A(1)*cos(t(i)) + B(1)*sin(t(i));
    y =  C(2) + A(2)*cos(t(i)) + B(2)*sin(t(i));
    z =  C(3) + A(3)*cos(t(i)) + B(3)*sin(t(i));
    p=[p;x y z];
end

selectnum = 50;
ind = randperm(length(p),selectnum);   
t = t(ind)';
data = p(ind,:) + (rand(selectnum,3)-0.5)/10;

plot3(p(:,1),p(:,2),p(:,3),'.');
grid on;
hold on;
plot3(data(:,1),data(:,2),data(:,3),'r*');

X = data(:,1);
Y = data(:,2);
Z = data(:,3);

prex = rand(3,1);
prey = rand(3,1);
prez = rand(3,1);
for i=1:500
    
    Fx = prex(1) + prex(2)*cos(t) + prex(3)*sin(t);
    Fy = prey(1) + prey(2)*cos(t) + prey(3)*sin(t);
    Fz = prez(1) + prez(2)*cos(t) + prez(3)*sin(t);
    
    Gx = X - Fx;
    Gy = Y - Fy;
    Gz = Z - Fz;
    
    Dc = ones(length(t),1);
    Da = cos(t);
    Db = sin(t);
    J = [Dc Da Db];
    
    deltax = inv(J'*J)*J'* Gx;
    deltay = inv(J'*J)*J'* Gy;
    deltaz = inv(J'*J)*J'* Gz;
    
    pcurx = prex+deltax;
    pcury = prey+deltay;
    pcurz = prez+deltaz;
    
    prex = pcurx;
    prey = pcury;
    prez = pcurz;
end

t = 0:0.01:2*pi;
p=[];
for i=1:length(t)
    x =  prex(1) + prex(2)*cos(t(i)) + prex(3)*sin(t(i));
    y =  prey(1) + prey(2)*cos(t(i)) + prey(3)*sin(t(i));
    z =  prez(1) + prez(2)*cos(t(i)) + prez(3)*sin(t(i));
    p=[p;x y z];
end

plot3(p(:,1),p(:,2),p(:,3),'go');

擬合結果：

藍色點為原始橢圓。

紅色星為藍色點中選50個點並且加入噪聲后的點。

綠色圈為擬合結果。