WIP: fit a 2D ellipse on 3D points + plot them.

The transform back is not correct yet.
See how to make the fit and rotation with respect to point centers,
then translate ellipse back at the right coordinates.

src/fit_ellipse_3d.m

+function [ f, v ] = fit_ellipse_3d( p, E, method )
+%FIT_ELLIPSE_3D Fit a 2D ellipse on the 3D points.
+%   The fit requires the Euler angles of the plane fitted through the
+%   opints, so that we can project them on this plane. We then make a 2D 
+%   ellipse fit on the projected points. This turns to be much more robust 
+%   than a 3D fit, and also closely match our configuration.
+
+%   Greatly inspired from https://stackoverflow.com/questions/29051168
+%   /data-fitting-an-ellipse-in-3d-space
+
+    if nargin < 3
+        method = 'direct';
+    end
+    
+    c = mean( p );
+    p = p - repmat( c, size( p, 1 ), 1 );
+
+    % Fit a plane to these points.
+    if nargin < 2
+        [ ~, ~, v ] = svd( p );
+    else
+        v = euleurZXZ2rot( E );
+    end
+    
+    % Rotate the points into the principal axes frame.
+    pt = p * v;
+
+    f = fit_ellipse_2d( pt( :, 1:2 ), method );
+    
+end
+

src/plot_ellipse_3d.m

+function h = plot_ellipse_3d( f, v, npoints)
+%PLOT_ELLIPSE_3D Plot a 2D ellipse in 3D.
+
+    if nargin < 3
+        npoints = 20;
+    end
+    
+    x0      = f(1);
+    y0      = f(2);
+    a       = f(3);
+    b       = f(4);
+    theta   = f(5);
+
+    R = [   cos( theta ) sin( theta ) ;
+        -sin( theta ) cos( theta ) ] ;
+    
+    t = linspace( 0 , 2 * pi, npoints )';
+    XY0 = [ a * sin(t), b * cos(t) ];
+    XY1 = XY0 * R;
+    xr = XY1( :, 1 ) + x0;
+    yr = XY1( :, 2 ) + y0;
+    zr = zeros( numel( xr ), 1 );
+    pr = [ xr yr zr ];
+    
+    % Transform back
+    pb = pr * v';
+    xb = pb( :, 1 );
+    yb = pb( :, 2 );
+    zb = pb( :, 3 );
+    
+    xb = [ xb ; xb(1,:) ];
+    yb = [ yb ; yb(1,:) ];
+    zb = [ zb ; zb(1,:) ];
+    h = line( xb, yb, zb );
+
+end
+