I have many points inside a square. I want to partition the square in many small rectangles and check how many points fall in each rectangle, i.e. I want to compute the joint probability distribution of the points. I am reporting a couple of common sense approaches, using loops and not very efficient:
% Data
N = 1e5; % number of points
xy = rand(N, 2); % coordinates of points
xy(randi(2*N, 100, 1)) = 0; % add some points on one side
xy(randi(2*N, 100, 1)) = 1; % add some points on the other side
xy(randi(N, 100, 1), :) = 0; % add some points on one corner
xy(randi(N, 100, 1), :) = 1; % add some points on one corner
inds= unique(randi(N, 100, 1)); xy(inds, :) = repmat([0 1], numel(inds), 1); % add some points on one corner
inds= unique(randi(N, 100, 1)); xy(inds, :) = repmat([1 0], numel(inds), 1); % add some points on one corner
% Intervals for rectangles
K1 = ceil(sqrt(N/5)); % number of intervals along x
K2 = K1; % number of intervals along y
int_x = [0:(1 / K1):1, 1+eps]; % intervals along x
int_y = [0:(1 / K2):1, 1+eps]; % intervals along y
% First approach
tic
count_cells = zeros(K1 + 1, K2 + 1);
for k1 = 1:K1+1
inds1 = (xy(:, 1) >= int_x(k1)) & (xy(:, 1) < int_x(k1 + 1));
for k2 = 1:K2+1
inds2 = (xy(:, 2) >= int_y(k2)) & (xy(:, 2) < int_y(k2 + 1));
count_cells(k1, k2) = sum(inds1 .* inds2);
end
end
toc
% Elapsed time is 46.090677 seconds.
% Second approach
tic
count_again = zeros(K1 + 2, K2 + 2);
for k1 = 1:K1+1
inds1 = (xy(:, 1) >= int_x(k1));
for k2 = 1:K2+1
inds2 = (xy(:, 2) >= int_y(k2));
count_again(k1, k2) = sum(inds1 .* inds2);
end
end
count_again_fix = diff(diff(count_again')');
toc
% Elapsed time is 22.903767 seconds.
% Check: the two solutions are equivalent
all(count_cells(:) == count_again_fix(:))
How can I do it more efficiently in terms of time, memory, and possibly avoiding loops?
EDIT --> I have just found this as well, it's the best solution found so far:
tic
count_cells_hist = hist3(xy, 'Edges', {int_x int_y});
count_cells_hist(end, :) = []; count_cells_hist(:, end) = [];
toc
all(count_cells(:) == count_cells_hist(:))
% Elapsed time is 0.245298 seconds.
but it requires the Statistics Toolbox.
EDIT --> Testing solution suggested by chappjc
tic
xcomps = single(bsxfun(#ge,xy(:,1),int_x));
ycomps = single(bsxfun(#ge,xy(:,2),int_y));
count_again = xcomps.' * ycomps; %' 143x143 = 143x1e5 * 1e5x143
count_again_fix = diff(diff(count_again')');
toc
% Elapsed time is 0.737546 seconds.
all(count_cells(:) == count_again_fix(:))
I have written a simple mex function which works very well when N is large. Of course it's cheating but still ...
The function is
#include "mex.h"
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
unsigned long int hh, ctrl; /* counters */
unsigned long int N, m, n; /* size of matrices */
unsigned long int *xy; /* data */
unsigned long int *count_cells; /* joint frequencies */
/* matrices needed */
mxArray *count_cellsArray;
/* Now we need to get the data */
if (nrhs == 3) {
xy = (unsigned long int*) mxGetData(prhs[0]);
N = (unsigned long int) mxGetM(prhs[0]);
m = (unsigned long int) mxGetScalar(prhs[1]);
n = (unsigned long int) mxGetScalar(prhs[2]);
}
/* Then build the matrices for the output */
count_cellsArray = mxCreateNumericMatrix(m + 1, n + 1, mxUINT32_CLASS, mxREAL);
count_cells = mxGetData(count_cellsArray);
plhs[0] = count_cellsArray;
hh = 0; /* counter for elements of xy */
/* for all points from 1 to N */
for(hh=0; hh<N; hh++) {
ctrl = (m + 1) * xy[N + hh] + xy[hh];
count_cells[ctrl] = count_cells[ctrl] + 1;
}
}
It can be saved in a file "joint_dist_points_2D.c", then compiled:
mex joint_dist_points_2D.c
And check it out:
% Data
N = 1e7; % number of points
xy = rand(N, 2); % coordinates of points
xy(randi(2*N, 1000, 1)) = 0; % add some points on one side
xy(randi(2*N, 1000, 1)) = 1; % add some points on the other side
xy(randi(N, 1000, 1), :) = 0; % add some points on one corner
xy(randi(N, 1000, 1), :) = 1; % add some points on one corner
inds= unique(randi(N, 1000, 1)); xy(inds, :) = repmat([0 1], numel(inds), 1); % add some points on one corner
inds= unique(randi(N, 1000, 1)); xy(inds, :) = repmat([1 0], numel(inds), 1); % add some points on one corner
% Intervals for rectangles
K1 = ceil(sqrt(N/5)); % number of intervals along x
K2 = ceil(sqrt(N/7)); % number of intervals along y
int_x = [0:(1 / K1):1, 1+eps]; % intervals along x
int_y = [0:(1 / K2):1, 1+eps]; % intervals along y
% Use Statistics Toolbox: hist3
tic
count_cells_hist = hist3(xy, 'Edges', {int_x int_y});
count_cells_hist(end, :) = []; count_cells_hist(:, end) = [];
toc
% Elapsed time is 4.414768 seconds.
% Use mex function
tic
xy2 = uint32(floor(xy ./ repmat([1 / K1, 1 / K2], N, 1)));
count_cells = joint_dist_points_2D(xy2, uint32(K1), uint32(K2));
toc
% Elapsed time is 0.586855 seconds.
% Check: the two solutions are equivalent
all(count_cells_hist(:) == count_cells(:))
Improving on code in question
Your loops (and the nested dot product) can be eliminated with bsxfun and matrix multiplication as follows:
xcomps = bsxfun(#ge,xy(:,1),int_x);
ycomps = bsxfun(#ge,xy(:,2),int_y);
count_again = double(xcomps).'*double(ycomps); %' 143x143 = 143x1e5 * 1e5x143
count_again_fix = diff(diff(count_again')');
The multiplication step accomplishes the AND and summation done in sum(inds1 .* inds2), but without looping over the density matrix. EDIT: If you use single instead of double, execution time is nearly halved, but be sure to convert your answer to double or whatever is required for the rest of the code. On my computer this takes around 0.5 sec.
Note: With rot90(count_again/size(xy,1),2) you have a CDF, and in rot90(count_again_fix/size(xy,1),2) you have a PDF.
Using accumarray
Another approach is to use accumarray to make the joint histogram after we bin the data.
Starting with int_x, int_y, K1, xy, etc.:
% take (0,1) data onto [1 K1], following A.Dondas approach for easy comparison
ii = floor(xy(:,1)*(K1-eps))+1; ii(ii<1) = 1; ii(ii>K1) = K1;
jj = floor(xy(:,2)*(K1-eps))+1; jj(jj<1) = 1; jj(jj>K1) = K1;
% create the histogram and normalize
H = accumarray([ii jj],ones(1,size(ii,1)));
PDF = H / size(xy,1); % for probabilities summing to 1
On my computer, this takes around 0.01 sec.
The output is the same as A.Donda's converted from sparse to full (full(H)). Although, as he A.Donda pointed out, it is correct to have the dimensions be K1xK1, rather than the size of count_again_fix in the OPs code that was K1+1xK1+1.
To get the CDF, I believe you can simply apply cumsum to each axis of the PDF.
chappjc's answer and using hist3 are all good, but since I happened to want to have something like this some time ago and for some reason didn't find hist3 I wrote it myself, and I thought I'd post it here as a bonus. It uses sparse to do the actual counting and returns the result as a sparse matrix, so it may be useful for dealing with a multimodal distribution where different modes are far apart – or for someone who doesn't have the Statistics Toolbox.
Application to francesco's data:
K1 = ceil(sqrt(N/5));
[H, xs, ys] = hist2d(xy(:, 1), xy(:, 2), [K1 K1], [0, 1 + eps, 0, 1 + eps]);
Called with output parameters the function just returns the result, without it makes a color plot.
Here's the function:
function [H, xs, ys] = hist2d(x, y, n, ax)
% plot 2d-histogram as an image
%
% hist2d(x, y, n, ax)
% [H, xs, ys] = hist2d(x, y, n, ax)
%
% x: data for horizontal axis
% y: data for vertical axis
% n: how many bins to use for each axis, default is [100 100]
% ax: axis limits for the plot, default is [min(x), max(x), min(y), max(y)]
% H: 2d-histogram as a sparse matrix, indices 1 & 2 correspond to x & y
% xs: corresponding vector of x-values
% ys: corresponding vector of y-values
%
% x and y have to be column vectors of the same size. Data points
% outside of the axis limits are allocated to the first or last bin,
% respectively. If output arguments are given, no plot is generated;
% it can be reproduced by "imagesc(ys, xs, H'); axis xy".
% defaults
if nargin < 3
n = [100 100];
end
if nargin < 4
ax = [min(x), max(x), min(y), max(y)];
end
% parameters
nx = n(1);
ny = n(2);
xl = ax(1 : 2);
yl = ax(3 : 4);
% generate histogram
i = floor((x - xl(1)) / diff(xl) * nx) + 1;
i(i < 1) = 1;
i(i > nx) = nx;
j = floor((y - yl(1)) / diff(yl) * ny) + 1;
j(j < 1) = 1;
j(j > ny) = ny;
H = sparse(i, j, ones(size(i)), nx, ny);
% generate axes
xs = (0.5 : nx) / nx * diff(xl) + xl(1);
ys = (0.5 : ny) / ny * diff(yl) + yl(1);
% possibly plot
if nargout == 0
imagesc(ys, xs, H')
axis xy
clear H xs ys
end
Related
I am trying to represent a double exponential function with Fourier series in Matlab, but without the dc component. The basic function has the form K*(exp(-t.*alpha) - exp(-t.*beta)) and the expected format should be like this double exponential with the time domain function. I used the Heaviside function to make the rectangular window and invert the waveform.
clear, clc
Tr = 1.5E-6; Tf = 50E-6;
al=1/Tf; be=1/Tr; % alpha and beta
f0 = 1/(8*Tf);
T = 1/f0; % fundamental period
K = 1; % Amplitude
t=0:Tr/4:T; % Time
w0 = 2*pi/T; % Fundamental freq (in rad/s)
uV = 1E-6; % 1 uV = 1E-6 V
%% Double exponential - TD continuous
DEXP_TD_parameters = K* (exp(-t.*al) - exp(-t.*be));
DEXP_TD = K* (exp(-t.*al) - exp(-t.*be)).* (heaviside(t)-heaviside(t-T/2)) ...
- K* (exp(-(t-T/2).*al) - exp(-(t-T/2).*be)).*(heaviside(t-T/2)-heaviside(t-T));
Then, I calculated the ck of the series and plotted the time domain function again.
%% DEXP - Fourier coefficient c_k
h = 150; % Number of harmonics
for m = 1:h
if mod(m,2) == 0, ck(m) = NaN;
else
ck(m) = (K/pi) * ( (1+exp(-al*pi/w0)) / (1j*m+al/w0) ...
-(1+exp(-be*pi/w0)) / (1j*m+be/w0) );
ck_n(h-m+1) = (K/pi) * ( (1+exp(-al*pi/w0)) / (1j*(-m)+al/w0) ...
-(1+exp(-be*pi/w0)) / (1j*(-m)+be/w0) );
end
end
ck_n(ck_n == 0) = NaN;
%% FD/TD Reconstruction
DEXP_TD_rec = zeros(1,(length(t)));
summ = zeros(1,h);
if mod(h,2) == 0
hk=h-1;
else hk=h;
end
for m = 1:length(t)
for k = 1:2:h(end)
% DEXP_sum(k) = (ck(k)) * (exp(1j*w0*k*t(m)) + exp(-1j*w0*k*t(m)));
DEXP_sum(k) = 2*abs(ck(k)) * (cos(k*w0.*t(m) + angle(ck(k))));
end
DEXP_TD_rec(m) = sum(DEXP_sum);
end
%% Plots
figure(1) % TD
plot(t/uV, DEXP_TD,'r', t/uV, abs(DEXP_TD_rec),'--b')
grid
The time domain Fourier series waveform (in blue) does not match the red curve, and I wonder why. The shape is okay, but the second half of the curve should be negative. Any tips on how to solve it?
I have a (real) 2D gpuArray, which I am using as part of a larger code, and now am trying to also integrate the array using the Composite Simpson Rule inside my main loop (several 10000 iterations at least). A MWE looks like the following:
%%%%%%%%%%%%%%%%%% MAIN CODE %%%%%%%%%%%%%%%%%%
Ny = 501; % Dimensions of matrix M
Nx = 503; %
dx = 0.1; % Grid spacings
dy = 0.2; %
M = rand(Ny, Nx, 'gpuArray'); % Initialise a matrix
for k = 1:10000
% M = function1(M) % Apply some other functions to M
% ... etc ...
I = simpsons_integration_2D(M, dx, dy, Nx, Ny); % Now integrate M
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%% Integrator %%%%%%%%%%%%%%%%%
function I = simpsons_integration_2D(F, dx, dy, Nx, Ny)
% Integrate the 2D function F with Nx columns and Ny rows, and grid spacings
% dx and dy using Simpson's rule.
% Integrate along x direction (vertically) --> IX is a vector afterwards
sX = sum( F(:,1:2:Nx-2) + 4*F(:,2:2:(Nx-1)) + F(:,3:2:Nx) , 2);
IX = dx/3 * sX;
% Integrate along y direction --> I is a scalar afterwards
sY = sum( IX(1:2:Ny-2) + 4*IX(2:2:(Ny-1)) + IX(3:2:Ny) , 1);
I = dy/3 * sY;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
The operation of performing the integration is around 850 µs, which is currently a significant part of my code. This was measured using
f = #() simpsons_integration_2D(M, dx, dy, Nx, Ny);
t = gputimeit(f)
Is there a way to reduce the execution time for integrating the gpuArray matrix?
(The graphics card is the Nvidia Quadro P4000)
Many thanks
Assuming that the matrix has odd dimensions here is a way to optimize the function:
function I = simpsons_integration_2D(F, dx, dy, Nx, Ny)
sX = 2 * sum(F,2) + 2 * sum (F(:,2:2:(Nx-1)),2) - F(:,1) - F(:,Nx);
sY = dx/3 * (2 * sum(sX) + 2 * sum (sX(2:2:(Ny-1))) - sX(1) - sX(Ny));
I = dy/3 * sY;
end
EDIT
A more optimized solution using matrix multiplication:
function I = simpsons_integration_2D2(F, dx, dy, Nx, Ny)
mx = repmat (2, Nx, 1);
mx(2:2:(Nx-1)) = 4;
mx(1) = 1;
mx(Nx) = 1;
my = repmat (2, 1, Ny);
my(2:2:(Ny-1)) = 4;
my(1) = 1;
my(Ny) = 1;
I = (dx*dy/9) * (my * (F * mx));
end
If Nx and Ny are the same you need to compute only one of them mx or my:
function I = simpsons_integration_2D2(F, dx, dy, Nx, Ny)
mx = repmat (2, Nx, 1);
mx(2:2:(Nx-1)) = 4;
mx(1) = 1;
mx(Nx) = 1;
I = (dx*dy/9) * (mx.' * (F * mx));
end
If Nx and Ny are constant you can precompute mx outside the function and pass it as a function argument:
function I = simpsons_integration_2D2(F, dx, dy, mx)
I = (dx*dy/9) * (mx.' * (F * mx));
end
EDIT:
If both mx and my can be precomputed the problem is reduced to a dot product:
m = reshape (my.' .* mx.', 1, []);
function I = simpsons_integration_2D3(F, dx, dy, m)
I = (dx*dy/9) * (m * F(:));
end
Well I cannot test this for you but there are a few things that may help.
First the axis 1 and then the the axis 2 may make some difference in terms of locality of the modified terms (I don't know if to better or to worse).
function I = variation1(F, dx, dy, Nx, Ny)
% Sum each term separately, prevents the creation of a big intermediate matrix
% Multiply outside the summation does only Ny multiplications by 4 instead of Ny*Nx/2
sX = sum(F(:,1:2:Nx-2), 2) + 4*sum(F(:,2:2:(Nx-1)), 2) + sum(F(:,3:2:Nx), 2);
IX = dx/3 * sX;
sY = sum(IX(1:2:Ny-2), 1) + 4*sum(IX(2:2:(Ny-1)), 1) + sum(IX(3:2:Ny) , 1);
I = dy/3 * sY;
end
function I = variation2(F, dx, dy, Nx, Ny)
% a.
% Sum each term separately, prevents the creation of a big intermediate matrix
% Multiply outside the summation does only Ny multiplications by 4 instead of Ny*Nx/2
% b.
% Notice that the terms 2:3:NX-2 appear in two summations
% Saves Nx*Ny/2 additions at the expense of Ny multiplications by 2
sX = 2*sum(F(:,3:2:Nx-2), 2) + 4*sum(F(:,2:2:(Nx-1)), 2) + F(:,1) + F(:,Nx);
% saves Ny multiplications by moving the constant factor after the next sum
sY = 2*sum(sX(3:2:Ny-2), 1) + 4*sum(sX(2:2:(Ny-1)), 1) + sX(1) + sX(Ny);
I = (dy*dy/9) * sY;
end
function I = alternate_simpsons_integration_2D(F, dx, dy, Nx, Ny)
% Integrate the 2D function F with Nx columns and Ny rows, and grid spacings
% dx and dy using Simpson's rule.
% Notice that sum(F(:,1:2:Nx-2) + F(:,3:2:Nx)) have all but the end poitns repeated.
IX = 4*sum(F(:,2:2:Nx-1), 2) + 2 * sum(F(:,3:2:Nx-2) , 2) + F(:,1) + F(:,Nx);
disp(size(IX))
% Integrate along y direction --> I is a scalar afterwards
sY = 4*sum(IX(2:2:Ny-1)) + 2*sum(IX(3:2:Ny-2)) + IX(1) + IX(Ny);
I = dy*dy/9 * sY;
end
If you think it is better to make a single summation then you can do using the formula 2*(sum(2*F(2:2:end-1) + F(1:2:end-2)) + F(end) - F(1) that gives the same result but has Nx*Ny/2 less additions on the first integration. But these options have to be tested in your environment.
Transposed implementation
function I = transposed_simpsons_integration_2D(F, dx, dy, Nx, Ny)
sY = 2*sum(2*F(2:2:end-1, :) + F(1:2:end-2, :), 1) + F(end, :) - F(1, :);
sX = 2*sum(2*sY(2:2:end-1) + sY(1:2:end-2)) + sY(end) - sY(1);
I = dy*dy/9 * sX;
end
Using octave (usually slower than matlab) I get a run time of ~400us per iteration with. This is not the type of workload that will be interesting to run on the GPU. For comparison, randn about 10 times slower than this function.
I have 2 features which I expand to contain all possible combinations of the two features under order 6. When I do MATLAB's fminunc, it returns a weight vector where all elements are 0.
The dataset is here
clear all;
clc;
data = load("P2-data1.txt");
m = length(data);
para = 0; % regularization parameter
%% Augment Feature
y = data(:,3);
new_data = newfeature(data(:,1), data(:,2), 3);
[~, n] = size(new_data);
betas1 = zeros(n,1); % initial weights
options = optimset('GradObj', 'on', 'MaxIter', 400);
[beta_new, cost] = fminunc(#(t)(regucostfunction(t, new_data, y, para)), betas1, options);
fprintf('Cost at theta found by fminunc: %f\n', cost);
fprintf('theta: \n');
fprintf(' %f \n', beta_new); % get all 0 here
% Compute accuracy on our training set
p_new = predict(beta_new, new_data);
fprintf('Train Accuracy after feature augmentation: %f\n', mean(double(p_new == y)) * 100);
fprintf('\n');
%% the functions are defined below
function g = sigmoid(z) % running properly
g = zeros(size(z));
g=ones(size(z))./(ones(size(z))+exp(-z));
end
function [J,grad] = regucostfunction(theta,x,y,para) % CalculateCost(x1,betas1,y);
m = length(y); % number of training examples
J = 0;
grad = zeros(size(theta));
hyp = sigmoid(x*theta);
err = (hyp - y)';
grad = (1/m)*(err)*x;
sum = 0;
for k = 2:length(theta)
sum = sum+theta(k)^2;
end
J = (1/m)*((-y' * log(hyp) - (1 - y)' * log(1 - hyp)) + para*(sum) );
end
function p = predict(theta, X)
m = size(X, 1); % Number of training examples
p = zeros(m, 1);
index = find(sigmoid(theta'*X') >= 0.5);
p(index,1) = 1;
end
function out = newfeature(X1, X2, degree)
out = ones(size(X1(:,1)));
for i = 1:degree
for j = 0:i
out(:, end+1) = (X1.^(i-j)).*(X2.^j);
end
end
end
data contains 2 columns of rows followed by a third column of 0/1 values.
The functions used are: newfeature returns the expanded features and regucostfunction computes the cost. When I did the same approach with the default features, it worked and I think the problem here has to do with some coding issue.
I am trying to simulate the rotation dynamics of a system. I am testing my code to verify that it's working using simulation, but I never recovered the parameters I pass to the model. In other words, I can't re-estimate the parameters I chose for the model.
I am using MATLAB for that and specifically ode45. Here is my code:
% Load the input-output data
[torque outputs] = DataLogs2();
u = torque;
% using the simulation data
Ixx = 1.00;
Iyy = 2.00;
Izz = 3.00;
x0 = [0; 0; 0];
Ts = .02;
t = 0:Ts:Ts * ( length(u) - 1 );
[ T, x ] = ode45( #(t,x) rotationDyn( t, x, u(1+floor(t/Ts),:), Ixx, Iyy, Izz), t, x0 );
w = x';
N = length(w);
q = 1; % a counter for the A and B matrices
% The Algorithm
for k=1:1:N
w_telda = [ 0 -w(3, k) w(2,k); ...
w(3,k) 0 -w(1,k); ...
-w(2,k) w(1,k) 0 ];
if k == N % to handle the problem of the last iteration
w_dash(:,k) = (-w(:,k))/Ts;
else
w_dash(:,k) = (w(:,k+1)-w(:,k))/Ts;
end
a = kron( w_dash(:,k)', eye(3) ) + kron( w(:,k)', w_telda );
A(q:q+2,:) = a; % a 3N*9 matrix
B(q:q+2,:) = u(k,:)'; % a 3N*1 matrix % u(:,k)
q = q + 3;
end
% Forcing J to be diagonal. This is the case when we consider our quadcopter as two thin uniform
% rods crossed at the origin with a point mass (motor) at the end of each.
A_new = [A(:, 1) A(:, 5) A(:, 9)];
vec_J_diag = A_new\B;
J_diag = diag([vec_J_diag(1), vec_J_diag(2), vec_J_diag(3)])
eigenvalues_J_diag = eig(J_diag)
error = norm(A_new*vec_J_diag - B)
where my dynamic model is defined as:
function [dw, y] = rotationDyn(t, w, tau, Ixx, Iyy, Izz, varargin)
% The output equation
y = [w(1); w(2); w(3)];
% State equation
% dw = (I^-1)*( tau - cross(w, I*w) );
dw = [Ixx^-1 * tau(1) - ((Izz-Iyy)/Ixx)*w(2)*w(3);
Iyy^-1 * tau(2) - ((Ixx-Izz)/Iyy)*w(1)*w(3);
Izz^-1 * tau(3) - ((Iyy-Ixx)/Izz)*w(1)*w(2)];
end
Practically, what this code should do, is to calculate the eigenvalues of the inertia matrix, J, i.e. to recover Ixx, Iyy, and Izz that I passed to the model at the very begining (1, 2 and 3), but all what I get is wrong results.
Is the problem with using ode45?
Well the problem wasn't in the ode45 instruction, the problem is that in system identification one can create an n-1 samples-signal from an n samples-signal, thus the loop has to end at N-1 in the above code.
I am trying to implement least squares circle fitting following this paper (sorry I can't publish it). The paper states, that we could fit a circle, by calculating the geometric error as the euclidean distance (Xi'') between a specific point (Xi) and the corresponding point on the circle (Xi'). We have three parametres: Xc (a vector of coordinates the center of circle), and R (radius).
I came up with the following MATLAB code (note that I am trying to fit circles, not spheres as it is indicated on the images):
function [ circle ] = fit_circle( X )
% Kör paraméterstruktúra inicializálása
% R - kör sugara
% Xc - kör középpontja
circle.R = NaN;
circle.Xc = [ NaN; NaN ];
% Kezdeti illesztés
% A köz középpontja legyen a súlypont
% A sugara legyen az átlagos négyzetes távolság a középponttól
circle.Xc = mean( X );
d = bsxfun(#minus, X, circle.Xc);
circle.R = mean(bsxfun(#hypot, d(:,1), d(:,2)));
circle.Xc = circle.Xc(1:2)+random('norm', 0, 1, size(circle.Xc));
% Optimalizáció
options = optimset('Jacobian', 'on');
out = lsqnonlin(#ort_error, [circle.Xc(1), circle.Xc(2), circle.R], [], [], options, X);
end
%% Cost function
function [ error, J ] = ort_error( P, X )
%% Calculate error
R = P(3);
a = P(1);
b = P(2);
d = bsxfun(#minus, X, P(1:2)); % X - Xc
n = bsxfun(#hypot, d(:,1), d(:,2)); % || X - Xc ||
res = d - R * bsxfun(#times,d,1./n);
error = zeros(2*size(X,1), 1);
error(1:2:2*size(X,1)) = res(:,1);
error(2:2:2*size(X,1)) = res(:,2);
%% Jacobian
xdR = d(:,1)./n;
ydR = d(:,2)./n;
xdx = bsxfun(#plus,-R./n+(d(:,1).^2*R)./n.^3,1);
ydy = bsxfun(#plus,-R./n+(d(:,2).^2*R)./n.^3,1);
xdy = (d(:,1).*d(:,2)*R)./n.^3;
ydx = xdy;
J = zeros(2*size(X,1), 3);
J(1:2:2*size(X,1),:) = [ xdR, xdx, xdy ];
J(2:2:2*size(X,1),:) = [ ydR, ydx, ydy ];
end
The fitting however is not too good: if I start with the good parameter vector the algorithm terminates at the first step (so there is a local minima where it should be), but if I perturb the starting point (with a noiseless circle) the fitting stops with very large errors. I am sure that I've overlooked something in my implementation.
For what it's worth, I implemented these methods in MATLAB a while ago. However, I did it apparently before I knew about lsqnonlin etc, as it uses a hand-implemented regression. This is probably slow, but may help to compare with your code.
function [x, y, r, sq_error] = circFit ( P )
%# CIRCFIT fits a circle to a set of points using least sqaures
%# P is a 2 x n matrix of points to be fitted
per_error = 0.1/100; % i.e. 0.1%
%# initial estimates
X = mean(P, 2)';
r = sqrt(mean(sum((repmat(X', [1, length(P)]) - P).^2)));
v_cen2points = zeros(size(P));
niter = 0;
%# looping until convergence
while niter < 1 || per_diff > per_error
%# vector from centre to each point
v_cen2points(1, :) = P(1, :) - X(1);
v_cen2points(2, :) = P(2, :) - X(2);
%# distacnes from centre to each point
centre2points = sqrt(sum(v_cen2points.^2));
%# distances from edge of circle to each point
d = centre2points - r;
%# computing 3x3 jacobean matrix J, and solvign matrix eqn.
R = (v_cen2points ./ [centre2points; centre2points])';
J = [ -ones(length(R), 1), -R ];
D_rXY = -J\d';
%# updating centre and radius
r_old = r; X_old = X;
r = r + D_rXY(1);
X = X + D_rXY(2:3)';
%# calculating maximum percentage change in values
per_diff = max(abs( [(r_old - r) / r, (X_old - X) ./ X ])) * 100;
%# prevent endless looping
niter = niter + 1;
if niter > 1000
error('Convergence not met in 1000 iterations!')
end
end
x = X(1);
y = X(2);
sq_error = sum(d.^2);
This is then run with:
X = [1 2 5 7 9 3];
Y = [7 6 8 7 5 7];
[x_centre, y_centre, r] = circFit( [X; Y] )
And plotted with:
[X, Y] = cylinder(r, 100);
scatter(X, Y, 60, '+r'); axis equal
hold on
plot(X(1, :) + x_centre, Y(1, :) + y_centre, '-b', 'LineWidth', 1);
Giving: