Lorenz Meier
10 years ago
8 changed files with 408 additions and 47 deletions
@ -0,0 +1,174 @@
@@ -0,0 +1,174 @@
|
||||
% Copyright (c) 2009, Yury Petrov |
||||
% All rights reserved. |
||||
% |
||||
% Redistribution and use in source and binary forms, with or without |
||||
% modification, are permitted provided that the following conditions are |
||||
% met: |
||||
% |
||||
% * Redistributions of source code must retain the above copyright |
||||
% notice, this list of conditions and the following disclaimer. |
||||
% * Redistributions in binary form must reproduce the above copyright |
||||
% notice, this list of conditions and the following disclaimer in |
||||
% the documentation and/or other materials provided with the distribution |
||||
% |
||||
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
||||
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
||||
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
||||
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
||||
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
||||
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
||||
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
||||
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
||||
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
||||
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
% POSSIBILITY OF SUCH DAMAGE. |
||||
% |
||||
|
||||
function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals ) |
||||
% |
||||
% Fit an ellispoid/sphere 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 - 0 fits an arbitrary ellipsoid (default), |
||||
% - 1 fits an ellipsoid with its axes along [x y z] axes |
||||
% - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad |
||||
% - 3 fits a sphere |
||||
% |
||||
% Output: |
||||
% * center - ellispoid center coordinates [xc; yc; zc] |
||||
% * ax - ellipsoid radii [a; b; c] |
||||
% * evecs - ellipsoid radii directions as columns of the 3x3 matrix |
||||
% * v - the 9 parameters describing the ellipsoid algebraically: |
||||
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 |
||||
% |
||||
% Author: |
||||
% Yury Petrov, Northeastern University, Boston, MA |
||||
% |
||||
|
||||
error( nargchk( 1, 3, nargin ) ); % check input arguments |
||||
if nargin == 1 |
||||
flag = 0; % default to a free ellipsoid |
||||
end |
||||
if flag == 2 && nargin == 2 |
||||
equals = 'xy'; |
||||
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 && flag == 0 |
||||
error( 'Must have at least 9 points to fit a unique ellipsoid' ); |
||||
end |
||||
if length( x ) < 6 && flag == 1 |
||||
error( 'Must have at least 6 points to fit a unique oriented ellipsoid' ); |
||||
end |
||||
if length( x ) < 5 && flag == 2 |
||||
error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' ); |
||||
end |
||||
if length( x ) < 3 && flag == 3 |
||||
error( 'Must have at least 4 points to fit a unique sphere' ); |
||||
end |
||||
|
||||
if flag == 0 |
||||
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 |
||||
D = [ x .* x, ... |
||||
y .* y, ... |
||||
z .* z, ... |
||||
2 * x .* y, ... |
||||
2 * x .* z, ... |
||||
2 * y .* z, ... |
||||
2 * x, ... |
||||
2 * y, ... |
||||
2 * z ]; % ndatapoints x 9 ellipsoid parameters |
||||
elseif flag == 1 |
||||
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 |
||||
D = [ x .* x, ... |
||||
y .* y, ... |
||||
z .* z, ... |
||||
2 * x, ... |
||||
2 * y, ... |
||||
2 * z ]; % ndatapoints x 6 ellipsoid parameters |
||||
elseif flag == 2 |
||||
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1, |
||||
% where A = B or B = C or A = C |
||||
if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) |
||||
D = [ y .* y + z .* z, ... |
||||
x .* x, ... |
||||
2 * x, ... |
||||
2 * y, ... |
||||
2 * z ]; |
||||
elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) |
||||
D = [ x .* x + z .* z, ... |
||||
y .* y, ... |
||||
2 * x, ... |
||||
2 * y, ... |
||||
2 * z ]; |
||||
else |
||||
D = [ x .* x + y .* y, ... |
||||
z .* z, ... |
||||
2 * x, ... |
||||
2 * y, ... |
||||
2 * z ]; |
||||
end |
||||
else |
||||
% fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1 |
||||
D = [ x .* x + y .* y + z .* z, ... |
||||
2 * x, ... |
||||
2 * y, ... |
||||
2 * z ]; % ndatapoints x 4 sphere parameters |
||||
end |
||||
|
||||
% solve the normal system of equations |
||||
v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) ); |
||||
|
||||
% find the ellipsoid parameters |
||||
if flag == 0 |
||||
% 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) -1 ]; |
||||
% find the center of the ellipsoid |
||||
center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(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( evals ) ); |
||||
else |
||||
if flag == 1 |
||||
v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; |
||||
elseif flag == 2 |
||||
if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) |
||||
v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; |
||||
elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) |
||||
v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; |
||||
else % xy |
||||
v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; |
||||
end |
||||
else |
||||
v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; |
||||
end |
||||
center = ( -v( 7:9 ) ./ v( 1:3 ) )'; |
||||
gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) ); |
||||
radii = ( sqrt( gam ./ v( 1:3 ) ) )'; |
||||
evecs = eye( 3 ); |
||||
end |
||||
|
||||
|
||||
@ -0,0 +1,77 @@
@@ -0,0 +1,77 @@
|
||||
% |
||||
% Tool for plotting mag data |
||||
% |
||||
% Reference values: |
||||
% telem> [cal] mag #0 off: x:0.15 y:0.07 z:0.14 Ga |
||||
% MATLAB: x:0.1581 y: 0.0701 z: 0.1439 Ga |
||||
% telem> [cal] mag #0 scale: x:1.10 y:0.97 z:1.02 |
||||
% MATLAB: 0.5499, 0.5190, 0.4907 |
||||
% |
||||
% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga |
||||
% MATLAB: x:-0.1827 y:0.1147 z:-0.0848 Ga |
||||
% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 |
||||
% MATLAB: 0.5122, 0.5065, 0.4915 |
||||
% |
||||
% |
||||
% User-guided values: |
||||
% |
||||
% telem> [cal] mag #0 off: x:0.12 y:0.09 z:0.14 Ga |
||||
% telem> [cal] mag #0 scale: x:0.88 y:0.99 z:0.95 |
||||
% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga |
||||
% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 |
||||
|
||||
close all; |
||||
clear all; |
||||
|
||||
plot_scale = 0.8; |
||||
|
||||
xmax = plot_scale; |
||||
xmin = -xmax; |
||||
ymax = plot_scale; |
||||
ymin = -ymax; |
||||
zmax = plot_scale; |
||||
zmin = -zmax; |
||||
|
||||
mag0_raw = load('../../mag0_raw3.csv'); |
||||
mag1_raw = load('../../mag1_raw3.csv'); |
||||
|
||||
mag0_cal = load('../../mag0_cal3.csv'); |
||||
mag1_cal = load('../../mag1_cal3.csv'); |
||||
|
||||
fm0r = figure(); |
||||
|
||||
mag0_x_scale = 0.88; |
||||
mag0_y_scale = 0.99; |
||||
mag0_z_scale = 0.95; |
||||
|
||||
plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); |
||||
[mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); |
||||
mag0_raw_center |
||||
mag0_raw_radii |
||||
axis([xmin xmax ymin ymax zmin zmax]) |
||||
viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]); |
||||
|
||||
fm1r = figure(); |
||||
plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); |
||||
[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); |
||||
center |
||||
radii |
||||
axis([xmin xmax ymin ymax zmin zmax]) |
||||
|
||||
fm0c = figure(); |
||||
plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); |
||||
[mag0_cal_center, mag0_cal_radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) .* mag0_x_scale mag1_raw(:,2) .* mag0_y_scale mag1_raw(:,3) .* mag0_z_scale] ); |
||||
mag0_cal_center |
||||
mag0_cal_radii |
||||
axis([xmin xmax ymin ymax zmin zmax]) |
||||
viscircles([0, 0], [mag0_cal_radii(3)]); |
||||
|
||||
fm1c = figure(); |
||||
plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); |
||||
axis([xmin xmax ymin ymax zmin zmax]) |
||||
[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); |
||||
viscircles([0, 0], [radii(3)]); |
||||
|
||||
mag0_x_scale_matlab = 1 / (mag0_cal_radii(1) / mag0_raw_radii(1)) |
||||
mag0_y_scale_matlab = 1 / (mag0_cal_radii(2) / mag0_raw_radii(2)) |
||||
mag0_z_scale_matlab = 1 / (mag0_cal_radii(3) / mag0_raw_radii(3)) |
||||
Loading…
Reference in new issue