-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathellipsoid_fit_new.m
202 lines (191 loc) · 7.18 KB
/
ellipsoid_fit_new.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
function [ center, radii, evecs, v, chi2 ] = ellipsoid_fit_new( X, equals )
%
% Fit an ellispoid/sphere/paraboloid/hyperboloid to a set of xyz data points:
%
% [center, radii, evecs, pars ] = ellipsoid_fit( X )
% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
%
% Parameters:
% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors
% * flag - '' or empty fits an arbitrary ellipsoid (default),
% - 'xy' fits a spheroid with x- and y- radii equal
% - 'xz' fits a spheroid with x- and z- radii equal
% - 'xyz' fits a sphere
% - '0' fits an ellipsoid with its axes aligned along [x y z] axes
% - '0xy' the same with x- and y- radii equal
% - '0xz' the same with x- and z- radii equal
%
% Output:
% * center - ellispoid or other conic center coordinates [xc; yc; zc]
% * radii - ellipsoid or other conic radii [a; b; c]
% * evecs - the radii directions as columns of the 3x3 matrix
% * v - the 10 parameters describing the ellipsoid / conic algebraically:
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz + J = 0
% * chi2 - residual sum of squared errors (chi^2), this chi2 is in the
% coordinate frame in which the ellipsoid is a unit sphere.
%
% Author:
% Yury Petrov, Oculus VR
% Date:
% September, 2015
%
narginchk( 1, 3 ) ; % check input arguments
if nargin == 1
equals = ''; % no constraints by default
end
if size( X, 2 ) ~= 3
error( 'Input data must have three columns!' );
else
x = X( :, 1 );
y = X( :, 2 );
z = X( :, 3 );
end
% need nine or more data points
if length( x ) < 9 && strcmp( equals, '' )
error( 'Must have at least 9 points to fit a unique ellipsoid' );
end
if length( x ) < 8 && ( strcmp( equals, 'xy' ) || strcmp( equals, 'xz' ) )
error( 'Must have at least 8 points to fit a unique ellipsoid with two equal radii' );
end
if length( x ) < 6 && strcmp( equals, '0' )
error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
end
if length( x ) < 5 && ( strcmp( equals, '0xy' ) || strcmp( equals, '0xz' ) )
error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two equal radii' );
end
if length( x ) < 4 && strcmp( equals, 'xyz' )
error( 'Must have at least 4 points to fit a unique sphere' );
end
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx +
% 2Hy + 2Iz + J = 0 and A + B + C = 3 constraint removing one extra
% parameter
if strcmp( equals, '' )
D = [ x .* x + y .* y - 2 * z .* z, ...
x .* x + z .* z - 2 * y .* y, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 9 ellipsoid parameters
elseif strcmp( equals, 'xy' )
D = [ x .* x + y .* y - 2 * z .* z, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 8 ellipsoid parameters
elseif strcmp( equals, 'xz' )
D = [ x .* x + z .* z - 2 * y .* y, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 8 ellipsoid parameters
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
elseif strcmp( equals, '0' )
D = [ x .* x + y .* y - 2 * z .* z, ...
x .* x + z .* z - 2 * y .* y, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 6 ellipsoid parameters
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
% where A = B or B = C or A = C
elseif strcmp( equals, '0xy' )
D = [ x .* x + y .* y - 2 * z .* z, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 5 ellipsoid parameters
elseif strcmp( equals, '0xz' )
D = [ x .* x + z .* z - 2 * y .* y, ...
2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 5 ellipsoid parameters
% fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
elseif strcmp( equals, 'xyz' )
D = [ 2 * x, ...
2 * y, ...
2 * z, ...
1 + 0 * x ]; % ndatapoints x 4 ellipsoid parameters
else
error( [ 'Unknown parameter value ' equals '!' ] );
end
% solve the normal system of equations
d2 = x .* x + y .* y + z .* z; % the RHS of the llsq problem (y's)
u = ( D' * D ) \ ( D' * d2 ); % solution to the normal equations
% find the residual sum of errors
% chi2 = sum( ( 1 - ( D * u ) ./ d2 ).^2 ); % this chi2 is in the coordinate frame in which the ellipsoid is a unit sphere.
% find the ellipsoid parameters
% convert back to the conventional algebraic form
if strcmp( equals, '' )
v(1) = u(1) + u(2) - 1;
v(2) = u(1) - 2 * u(2) - 1;
v(3) = u(2) - 2 * u(1) - 1;
v( 4 : 10 ) = u( 3 : 9 );
elseif strcmp( equals, 'xy' )
v(1) = u(1) - 1;
v(2) = u(1) - 1;
v(3) = -2 * u(1) - 1;
v( 4 : 10 ) = u( 2 : 8 );
elseif strcmp( equals, 'xz' )
v(1) = u(1) - 1;
v(2) = -2 * u(1) - 1;
v(3) = u(1) - 1;
v( 4 : 10 ) = u( 2 : 8 );
elseif strcmp( equals, '0' )
v(1) = u(1) + u(2) - 1;
v(2) = u(1) - 2 * u(2) - 1;
v(3) = u(2) - 2 * u(1) - 1;
v = [ v(1) v(2) v(3) 0 0 0 u( 3 : 6 )' ];
elseif strcmp( equals, '0xy' )
v(1) = u(1) - 1;
v(2) = u(1) - 1;
v(3) = -2 * u(1) - 1;
v = [ v(1) v(2) v(3) 0 0 0 u( 2 : 5 )' ];
elseif strcmp( equals, '0xz' )
v(1) = u(1) - 1;
v(2) = -2 * u(1) - 1;
v(3) = u(1) - 1;
v = [ v(1) v(2) v(3) 0 0 0 u( 2 : 5 )' ];
elseif strcmp( equals, 'xyz' )
v = [ -1 -1 -1 0 0 0 u( 1 : 4 )' ];
end
v = v';
% form the algebraic form of the ellipsoid
A = [ v(1) v(4) v(5) v(7); ...
v(4) v(2) v(6) v(8); ...
v(5) v(6) v(3) v(9); ...
v(7) v(8) v(9) v(10) ];
% find the center of the ellipsoid
center = -A( 1:3, 1:3 ) \ v( 7:9 );
% form the corresponding translation matrix
T = eye( 4 );
T( 4, 1:3 ) = center';
% translate to the center
R = T * A * T';
% solve the eigenproblem
[ evecs, evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
radii = sqrt( 1 ./ diag( abs( evals ) ) );
sgns = sign( diag( evals ) );
radii = radii .* sgns;
% calculate difference of the fitted points from the actual data normalized by the conic radii
d = [ x - center(1), y - center(2), z - center(3) ]; % shift data to origin
d = d * evecs; % rotate to cardinal axes of the conic;
d = [ d(:,1) / radii(1), d(:,2) / radii(2), d(:,3) / radii(3) ]; % normalize to the conic radii
chi2 = sum( abs( 1 - sum( d.^2 .* repmat( sgns', size( d, 1 ), 1 ), 2 ) ) );
if abs( v(end) ) > 1e-6
v = -v / v(end); % normalize to the more conventional form with constant term = -1
else
v = -sign( v(end) ) * v;
end