Forum
I've got a set of data that apparently forms an ellipse in 3D space (not an ellipsoid, but a curve in 3D).
Being inspired by following thread http://au.mathworks.com/matlabcentral/newsreader/view_thread/65773
and with the help from someone ,I manage to get the optimization code running and outputs a set of best parameters x (vector). However, when I try to use this x to replicate the ellipse,the outcomes is a strange straight line in the space. I have been stacked on this for days., still don't know what went wrong....pretty much devastated... I hope someone can shed some light on this. The Mathematica formulations for the ellipse follows the same as in the above thread ,where
The 3D-ellipse is given by: (x;y;z) = (z1;z2;z3) + R(alpha,beta,gamma).(acos(phi); b*sin(phi);0)
where:
* z is the translation vector.
* R is the rotation matrix (using Euler angles, we first rotate alpha rad around the x-axis, then beta rad around the y-axis and finally gamma rad around the z-axis again).
* a is the long axis of the ellipse
* b is the short axis of the ellipse.
Here is my target function for optimization (ellipsefit.m)
function [merit]= ellipsefit(x, vmatrix) % x is the initial parameters, vmatrix stores the datapoints
load vmatrix.txt % In vmatrix, the data are stored: N rows x 3 columns
a = x(1);
b = x(2);c
alpha = x(3);
beta = x(4);
gamma = x(5);
z = [x(6),x(7),x(8)];
t = z'
[dim1, dim2]=size(vmatrix);
% construction of rotation matrix R(alpha,beta,gamma)
R1 = [cos(alpha), sin(alpha), 0; -sin(alpha), cos(alpha), 0; 0, 0, 1];v
R2 = [1, 0, 0; 0, cos(beta), sin(beta); 0, -sin(beta), cos(beta)];
R3 = [cos(gamma), sin(gamma), 0; -sin(gamma), cos(gamma), 0; 0, 0, 1];
R = R3*R2*R1;
% first compute vector phi (in the length of the data) by minimizing for every
% point in the data set the distance of this point to the ellipse
% (with its initial parameters a,b,alpha,beta,gamma, z1,z2,z3 held fixed) with respect to phi.
for i=1:dim1
point=vmatrix(i,:)';
dist=#(phi)sum((R*[a*cos(phi); b*sin(phi); 0]+t-point)).^2;
phi(i)=fminbnd(dist,0,2*pi);
end
v = [a*cos(phi); b*sin(phi); zeros(size(phi))];
P = R*v;
%The targetfunction is: g = (xi1,xi2,xi3)' -(z1,z2,z3)'-R(alpha,beta,gamma)(a cos(phi), b sin(phi),0)'
% Construction of distance function
merit = [vmatrix(:,1)-z(1)-P(1),vmatrix(:,2)-z(2)-P(2),vmatrix(:,3)-z(3)-P(3)];
merit = sqrt(sum(sum(merit.^2)))
end
here is the main function for parameters initialization and call for opts (xfit.m)
function [y] = xfit (x)
x= [1 1 1 1 1 1 1 1] % initial parameters
[x] = fminsearch(#ellipsefit,x) % set the distance minimum as the target function
y=x
end
code to reconstruct the ellipse in scatter points (ellipsescatter.txt)
x= [0.655,0.876,1.449,2.248,1.024,0.201,-0.11,0.002] % values obtained according to above routines
a = x(1);
b = x(2);
alpha = x(3);
beta = x(4);
gamma = x(5);
z = [x(6),x(7),x(8)];
R1 = [cos(alpha), sin(alpha), 0; -sin(alpha), cos(alpha), 0; 0, 0, 1];
R2 = [1, 0, 0; 0, cos(beta), sin(beta); 0, -sin(beta), cos(beta)];
R3 = [cos(gamma), sin(gamma), 0; -sin(gamma), cos(gamma), 0; 0, 0, 1];
R = R3*R2*R1;
phi=linspace(0,2*pi,100)
v = [a*cos(phi); b*sin(phi); zeros(size(phi))];
P = R*v;
u = P'
and last the data points (vmatrix)
0.002037965 0.004225765 0.002020202
0.005766671 0.007269193 0.004040404
0.010004802 0.00995638 0.006060606
0.014444336 0.012502725 0.008080808
0.019083408 0.014909533 0.01010101
0.023967745 0.017144827 0.012121212
0.03019849 0.01969697 0.014591289
0.038857283 0.022727273 0.017839321
0.045443501 0.024730475 0.02020202
0.051213405 0.026346492 0.022222222
0.061038174 0.028787879 0.02555121
0.069408829 0.030575164 0.028282828
0.075785861 0.031818182 0.030321465
0.088818543 0.033954681 0.034343434
0.095538223 0.03490652 0.036363636
0.109421234 0.036499949 0.04040404
0.123800737 0.037746182 0.044444444
0.131206601 0.038218171 0.046464646
0.146438211 0.038868525 0.050505051
0.162243245 0.039117883 0.054545455
0.178662839 0.03893748 0.058585859
0.195740664 0.038296774 0.062626263
0.204545539 0.037790433 0.064646465
0.222781268 0.036340005 0.068686869
0.23715887 0.034848485 0.071748051
0.251787024 0.033009003 0.074747475
0.26196429 0.031542949 0.076767677
0.278510276 0.028787879 0.079919236
0.294365342 0.025757576 0.082799669
0.306221108 0.023197784 0.084848485
0.31843759 0.020305704 0.086868687
0.331291367 0.016967964 0.088888889
0.342989936 0.013636364 0.090622484
0.352806191 0.010606061 0.091993214
0.36201461 0.007575758 0.093211986
0.376385537 0.002386324 0.094949495
0.386214665 -0.001515152 0.096012
0.396173756 -0.005800677 0.096969697
0.406365393 -0.010606061 0.097799682
0.417897899 -0.016666667 0.098516141
0.428059375 -0.022727273 0.098889844
0.436894505 -0.028787879 0.09893196
0.444444123 -0.034848485 0.098652697
0.45074522 -0.040909091 0.098061305
0.455830971 -0.046969697 0.097166076
0.457867157 -0.05 0.096591789
0.46096663 -0.056060606 0.095199991
0.461974832 -0.059090909 0.094368708
0.462821268 -0.063709158 0.092929293
0.46279206 -0.068181818 0.091323015
0.462224312 -0.071212121 0.090097745
0.461247257 -0.074242424 0.088770148
0.459194871 -0.07812596 0.086868687
0.456406121 -0.0818267 0.084848485
0.45309565 -0.085162601 0.082828283
0.449335762 -0.088184223 0.080808081
0.445185841 -0.090933095 0.078787879
0.440695103 -0.093443633 0.076767677
0.435904796 -0.095744683 0.074747475
0.429042582 -0.098484848 0.072052312
0.419877272 -0.101489369 0.068686869
0.41402731 -0.103049401 0.066666667
0.407719192 -0.104545455 0.064554798
0.395265308 -0.106881864 0.060606061
0.388611992 -0.107880111 0.058585859
0.374697979 -0.10945186 0.054545455
0.360058411 -0.11051623 0.050505051
0.352443612 -0.11084211 0.048484848
0.336646801 -0.111097219 0.044444444
0.320085063 -0.110817414 0.04040404
0.31150078 -0.110465333 0.038383838
0.293673303 -0.109300395 0.034343434
0.275417637 -0.107575758 0.030396076
0.265228963 -0.106361993 0.028282828
0.251914589 -0.104545455 0.025603647
0.234385536 -0.101745907 0.022222222
0.223443994 -0.099745394 0.02020202
0.212154519 -0.097501571 0.018181818
0.20046153 -0.09497557 0.016161616
0.188298809 -0.092121085 0.014141414
0.17558878 -0.088883868 0.012121212
0.162241674 -0.085201142 0.01010101
0.148154337 -0.081000773 0.008080808
0.136529019 -0.077272727 0.006507255
0.127611912 -0.074242424 0.005361311
0.116762238 -0.070350086 0.004040404
0.103195122 -0.065151515 0.002507114
0.095734279 -0.062121212 0.001725236
0.081719652 -0.056060606 0.000388246
0 0 0
This answer is not a direct fit in 3D, it instead involves first a rotation of the data so that the plane of the points coincides with the xy plane, then a fit to the data in 2D.
% input: data, a N x 3 array with one set of Cartesian coords per row
% remove the center of mass
CM = mean(data);
datap = data - ones(size(data,1),1)*CM;
% now rotate all points into the xy plane ...
% start by finding the plane:
[u s v]=svd(datap);
% rotate the data into the principal axes frame:
datap = datap*v;
% fit the equation for an ellipse to the rotated points
x= [0.25 0.07 0.037 0 0]'; % initial parameters
options=1;
xopt = fmins(#fellipse,x,options,[],datap) % set the distance minimum as the target function
This is the function fellipse, based on the function provided:
function [merit]= fellipse(x,data) % x is the initial parameters, data stores the datapoints
a = x(1);
b = x(2);
alpha = x(3);
z = x(4:5);
R = [cos(alpha), sin(alpha), 0; -sin(alpha), cos(alpha), 0; 0, 0, 1];
data = data*R;
merit = 0;
[dim1, dim2]=size(data);
for i=1:dim1
dist=#(phi)sum( ( [a*cos(phi);b*sin(phi)] + z - data(i,1:2)').^2 );
phi=fminbnd(dist,0,2*pi);
merit = merit+dist(phi);
end
end
Also, note again that this can be turned into a fit directly in 3D, but this answer is just as good if you can assume the data points lie approx. in a 2D plane. The current solution is likely much more efficient then a solution in 3D with additional parameters.
Hopefully the code is self-explanatory. I recommend looking at the link included in the OP, it explains the purpose of the loop over phi.
And this is how you can inspect the result of the fit:
a = xopt(1);
b = xopt(2);
alpha = xopt(3);
z = [xopt(4:5) ; 0]';
phi = linspace(0,2*pi,100)';
simdat = [a*cos(phi) b*sin(phi) zeros(size(phi))];
R = [cos(alpha), -sin(alpha), 0; sin(alpha), cos(alpha), 0; 0, 0, 1];
simdat = simdat*R + ones(size(simdat,1), 1)*z ;
figure, hold on
plot3(datap(:,1),datap(:,2),datap(:,3),'o')
plot3(simdat(:,1),simdat(:,2),zeros(size(simdat,1),1),'r-')
Edit
The following is a 3D approach. It does not appear to be very robust as it is quite sensitive to the choice of starting parameters. Some improvements may be necessary.
CM = mean(data);
datap = data - ones(size(data,1),1)*CM;
xopt = [ 0.07 0.25 1 -0.408976 0.610120 0 0 0]';
options=1;
xopt = fmins(#fellipse3d,xopt,options,[],datap) % set the distance minimum as the target function
The function fellipse3d is
function [merit]= fellipse3d(x,data) % x is the initial parameters, data stores the datapoints
a = abs(x(1));
b = abs(x(2));
alpha = x(3);
beta = x(4);
gamma = x(5);
z = x(6:8)';
[dim1, dim2]=size(data);
R1 = [cos(alpha), sin(alpha), 0; -sin(alpha), cos(alpha), 0; 0, 0, 1];
R2 = [1, 0, 0; 0, cos(beta), sin(beta); 0, -sin(beta), cos(beta)];
R3 = [cos(gamma), sin(gamma), 0; -sin(gamma), cos(gamma), 0; 0, 0, 1];
R = R3*R2*R1;
data = (data - z(ones(dim1,1),:))*R;
merit = 0;
for i=1:dim1
dist=#(phi)sum( ([a*cos(phi);b*sin(phi);0] - data(i,:)').^2 );
phi=fminbnd(dist,0,2*pi);
merit = merit+dist(phi);
end
end
You can visualize the results with
a = xopt(1);
b = xopt(2);
alpha = -xopt(3);
beta = -xopt(4);
gamma = -xopt(5);
z = xopt(6:8)' + CM;
dim1 = 100;
phi = linspace(0,2*pi,dim1)';
simdat = [a*cos(phi) b*sin(phi) zeros(size(phi))];
R1 = [cos(alpha), sin(alpha), 0; ...
-sin(alpha), cos(alpha), 0; ...
0, 0, 1];
R2 = [1, 0, 0; ...
0, cos(beta), sin(beta); ...
0, -sin(beta), cos(beta)];
R3 = [cos(gamma), sin(gamma), 0; ...
-sin(gamma), cos(gamma), 0; ...
0, 0, 1];
R = R1*R2*R3;
simdat = simdat*R + z(ones(dim1,1),:);
figure, hold on
plot3(data(:,1),data(:,2),data(:,3),'o')
plot3(simdat(:,1),simdat(:,2),simdat(:,3),'r-')
Related
I'm having serious problems in a simple example of fem assembly.
I just want to assemble the Mass matrix without any coefficient. The geometry is simple:
conn=[1, 2, 3];
p = [0 0; 1 0; 0 1];
I made it like this so that the physical element will be equal to the reference one.
my basis functions:
phi_1 = #(eta) 1 - eta(1) - eta(2);
phi_2 = #(eta) eta(1);
phi_3 = #(eta) eta(2);
phi = {phi_1, phi_2, phi_3};
Jacobian matrix:
J = #(x,y) [x(2) - x(1), x(3) - x(1);
y(2) - y(1), y(3) - y(1)];
The rest of the code:
M = zeros(np,np);
for K = 1:size(conn,1)
l2g = conn(K,:); %local to global mapping
x = p(l2g,1); %node x-coordinate
y = p(l2g,2); %node y-coordinate
jac = J(x,y);
loc_M = localM(jac, phi);
M(l2g, l2g) = M(l2g, l2g) + loc_M; %add element masses to M
end
localM:
function loc_M = localM(J,phi)
d_J = det(J);
loc_M = zeros(3,3);
for i = 1:3
for j = 1:3
loc_M(i,j) = d_J * quadrature(phi{i}, phi{j});
end
end
end
quadrature:
function value = quadrature(phi_i, phi_j)
p = [1/3, 1/3;
0.6, 0.2;
0.2, 0.6;
0.2, 0.2];
w = [-27/96, 25/96, 25/96, 25/96];
res = 0;
for i = 1:size(p,1)
res = res + phi_i(p(i,:)) * phi_j(p(i,:)) * w(i);
end
value = res;
end
For the simple entry (1,1) I obtain 0.833, while computing the integral by hand or on wolfram alpha I get 0.166 (2 times the result of the quadrature).
I tried with different points and weights for quadrature, but really I do not know what I am doing wrong.
Working on a little project to do with computer graphics. So far I have (I think) everything in order as the code below patches my world to the screen fine.
However, I have a final hurdle to jump: I would like the code to patch for different values of theta. In the code, it is set at 2*pi/4 but I would like to iterate and patch for every angle between 0:pi/4:2*pi. However, when I try to put the code in a for or while loop it doesn't seem to do what I expect, that is, to patch with one angle, then patch with another etc.
Really stuck I have tried a lot of stuff and now I'm just without any ideas. Would really appreciate any help or suggestions.
function world()
% Defining House Vertices
house_verts = [-5, 0, -5;
5, 0, -5;
5, 10, -5;
0,15,-5;
-5,10,-5;
-5,0,5;
5,0,5;
5,10,5;
0,15,5;
-5,10,5];
% Sorting out the homeogenous co-ordinates
ones = [1,1,1,1,1,1,1,1,1,1];
ones=transpose(ones);
house_verts = [house_verts, ones];
house_verts = transpose(house_verts);
% House faces
house_faces = [1,2,3,4,5;
2,7,8,3,3;
6,7,8,9,10;
1,6,10,5,5;
3,4,9,8,8;
4,5,10,9,9;
1,2,7,6,6];
world_pos = [];
% creating a street
street_vector = [1,0,1]; % the direction of the street
orthog_street_vector = [-1,0,1];
for i = 1:15
% current_pos1 and 2 will be the positions of the two houses
% opposite each other on the street
current_pos1 = 30*i*street_vector + 50*orthog_street_vector;
current_pos2 = 30*i*street_vector - 50*orthog_street_vector;
world_pos = [world_pos;current_pos1;current_pos2];
end
% initialising world vertices and faces
world_verts = [];
world_faces = [];
% Populating the street
for i =1:size(world_pos,1)
T = transmatrix(world_pos(i,:)); % a translation matrix
s = [1,1/2 + rand(),1];
S=scalmatrix(s); % a matrix for a random scaling of the height (y-coordinate)
Ry = rotymatrix(rand()*2*pi); % a matrix for a random rotation about the y-axis
A = T*Ry*S; % the compound transformation matrix to take the house into the world
obj_faces = size(world_verts,2) + house_faces; %increments the basic house faces to match the current object
obj_verts = A*house_verts;
world_verts = [world_verts, obj_verts]; % adds the vertices to the world
world_faces = [world_faces; obj_faces]; % adds the faces to the world
end
% initialising aligned vertices
align_verts = [];
% Aligning the vertices to the particular camera at angle theta
for elm = world_verts
x = 350 + 350*cos(2*pi/4);
z = 350 + 350*sin(2*pi/4);
y = 80;
u = [x,y,z];
v = [250,0,250];
d = v - u;
phiy = atan2(d(1),d(3));
phix = -atan2(d(2),sqrt(d(1)^2+d(3)^2));
T = transmatrix([-u(1),-u(2),-u(3)]);
Ry = rotymatrix(phiy);
Rx = rotxmatrix(phix);
A = Rx*Ry*T;
align_verts = [align_verts, A*elm];
end
% initialising projected vertices
proj_verts=[];
% Performing the projection
for elm = align_verts
proj = projmatrix(10);
projverts = proj*elm;
projverts = ((10/projverts(3))*projverts);
proj_verts = [proj_verts,projverts];
end
% Displaying the world
for i = 1:size(world_faces,1)
for j = 1:size(world_faces,2)
x(j) =proj_verts(1,world_faces(i,j));
z(j) = proj_verts(2,world_faces(i,j));
end
patch(x,z,'w')
end
end
function T = transmatrix(p)
T = [1 0 0 p(1) ; 0 1 0 p(2) ; 0 0 1 p(3) ; 0 0 0 1];
end
function S = scalmatrix(s)
S = [s(1) 0 0 0 ; 0 s(2) 0 0 ; 0 0 s(3) 0 ; 0 0 0 1];
end
function Ry = rotymatrix(theta)
Ry = [cos(theta), 0, -sin(theta),0;
0,1,0,0;
sin(theta),0,cos(theta),0;
0,0,0,1];
end
function Rx = rotxmatrix(phi)
Rx = [1, 0, 0, 0;
0, cos(phi), -sin(phi), 0;
0, sin(phi), cos(phi), 0;
0, 0, 0, 1];
end
function P = projmatrix(f)
P = [1,0,0,0
0,1,0,0
0,0,1,0
0,0,1/f,0];
end
Updated code: managed to get the loop to work but now there is some bug i don't understand when it does a full rotation again any help would be great.
function world()
% Defining House Vertices
house_verts = [-5, 0, -5;
5, 0, -5;
5, 10, -5;
0,15,-5;
-5,10,-5;
-5,0,5;
5,0,5;
5,10,5;
0,15,5;
-5,10,5];
% Sorting out the homeogenous co-ordinates
ones = [1,1,1,1,1,1,1,1,1,1];
ones=transpose(ones);
house_verts = [house_verts, ones];
house_verts = transpose(house_verts);
% House faces
house_faces = [1,2,3,4,5;
2,7,8,3,3;
6,7,8,9,10;
1,6,10,5,5;
3,4,9,8,8;
4,5,10,9,9;
1,2,7,6,6];
world_pos = [];
% creating a street
street_vector = [1,0,1]; % the direction of the street
orthog_street_vector = [-1,0,1];
for i = 1:15
% current_pos1 and 2 will be the positions of the two houses
% opposite each other on the street
current_pos1 = 30*i*street_vector + 50*orthog_street_vector;
current_pos2 = 30*i*street_vector - 50*orthog_street_vector;
world_pos = [world_pos;current_pos1;current_pos2];
end
% initialising world vertices and faces
world_verts = [];
world_faces = [];
% Populating the street
for i =1:size(world_pos,1)
T = transmatrix(world_pos(i,:)); % a translation matrix
s = [1,1/2 + rand(),1];
S=scalmatrix(s); % a matrix for a random scaling of the height (y-coordinate)
Ry = rotymatrix(rand()*2*pi); % a matrix for a random rotation about the y-axis
A = T*Ry*S; % the compound transformation matrix to take the house into the world
obj_faces = size(world_verts,2) + house_faces; %increments the basic house faces to match the current object
obj_verts = A*house_verts;
world_verts = [world_verts, obj_verts]; % adds the vertices to the world
world_faces = [world_faces; obj_faces]; % adds the faces to the world
end
% initialising aligned vertices
align_verts = [];
% initialising projected vertices
proj_verts=[];
% Aligning the vertices to the particular camera at angle theta
theta = 0;
t = 0;
while t < 100
proj_verts=[];
align_verts = [];
for elm = world_verts
x = 300 + 300*cos(theta);
z = 300 + 300*sin(theta);
y = 80;
u = [x,y,z];
v = [200,0,200];
d = v - u;
phiy = atan2(d(1),d(3));
phix = -atan2(d(2),sqrt(d(1)^2+d(3)^2));
T = transmatrix([-u(1),-u(2),-u(3)]);
Ry = rotymatrix(phiy);
Rx = rotxmatrix(phix);
A = Rx*Ry*T;
align_verts = [align_verts, A*elm];
end
% Performing the projection
for elm = align_verts
proj = projmatrix(6);
projverts = proj*elm;
projverts = ((6/projverts(3))*projverts);
proj_verts = [proj_verts,projverts];
end
% Displaying the world
for i = 1:size(world_faces,1)
for j = 1:size(world_faces,2)
x(j) = proj_verts(1,world_faces(i,j));
z(j) = proj_verts(2,world_faces(i,j));
end
patch(x,z,'w')
pbaspect([1,1,1]); % adjusts the aspect ratio of the figure
end
title('Projected Space', 'fontsize', 16, 'interpreter', 'latex')
xlabel('$x$', 'fontsize', 16, 'interpreter', 'latex')
ylabel('$z$', 'fontsize', 16, 'interpreter', 'latex')
zlabel('$y$', 'fontsize', 16, 'interpreter', 'latex')
axis([-5,5,-5,2,0,5]) % sets the axes limits
view(0,89)
pause(0.0000001)
theta = theta + 0.01;
clf
end
end
function T = transmatrix(p)
T = [1 0 0 p(1) ; 0 1 0 p(2) ; 0 0 1 p(3) ; 0 0 0 1];
end
function S = scalmatrix(s)
S = [s(1) 0 0 0 ; 0 s(2) 0 0 ; 0 0 s(3) 0 ; 0 0 0 1];
end
function Ry = rotymatrix(theta)
Ry = [cos(theta), 0, -sin(theta),0;
0,1,0,0;
sin(theta),0,cos(theta),0;
0,0,0,1];
end
function Rx = rotxmatrix(phi)
Rx = [1, 0, 0, 0;
0, cos(phi), -sin(phi), 0;
0, sin(phi), cos(phi), 0;
0, 0, 0, 1];
end
function P = projmatrix(f)
P = [1,0,0,0
0,1,0,0
0,0,1,0
0,0,1/f,0];
end
I want to move a red star marker along the spiral trajectory with an equal distance of 5 units between the red star points on its circumference like in the below image.
vertspacing = 10;
horzspacing = 10;
thetamax = 10*pi;
% Calculation of (x,y) - underlying archimedean spiral.
b = vertspacing/2/pi;
theta = 0:0.01:thetamax;
x = b*theta.*cos(theta)+50;
y = b*theta.*sin(theta)+50;
% Calculation of equidistant (xi,yi) points on spiral.
smax = 0.5*b*thetamax.*thetamax;
s = 0:horzspacing:smax;
thetai = sqrt(2*s/b);
xi = b*thetai.*cos(thetai);
yi = b*thetai.*sin(thetai);
plot(x,y,'b-');
hold on
I want to get a figure that looks like the following:
This is my code for the circle trajectory:
% Initialization steps.
format long g;
format compact;
fontSize = 20;
r1 = 50;
r2 = 35;
r3= 20;
xc = 50;
yc = 50;
% Since arclength = radius * (angle in radians),
% (angle in radians) = arclength / radius = 5 / radius.
deltaAngle1 = 5 / r1;
deltaAngle2 = 5 / r2;
deltaAngle3 = 5 / r3;
theta1 = 0 : deltaAngle1 : (2 * pi);
theta2 = 0 : deltaAngle2 : (2 * pi);
theta3 = 0 : deltaAngle3 : (2 * pi);
x1 = r1*cos(theta1) + xc;
y1 = r1*sin(theta1) + yc;
x2 = r2*cos(theta2) + xc;
y2 = r2*sin(theta2) + yc;
x3 = r3*cos(theta3) + xc;
y3 = r3*sin(theta3) + yc;
plot(x1,y1,'color',[1 0.5 0])
hold on
plot(x2,y2,'color',[1 0.5 0])
hold on
plot(x3,y3,'color',[1 0.5 0])
hold on
% Connecting Line:
plot([70 100], [50 50],'color',[1 0.5 0])
% Set up figure properties:
% Enlarge figure to full screen.
set(gcf, 'Units', 'Normalized', 'OuterPosition', [0, 0, 1, 1]);
drawnow;
axis square;
for i = 1 : length(theta1)
plot(x1(i),y1(i),'r*')
pause(0.1)
end
for i = 1 : length(theta2)
plot(x2(i),y2(i),'r*')
pause(0.1)
end
for i = 1 : length(theta3)
plot(x3(i),y3(i),'r*')
pause(0.1)
end
I can't think of a way to compute distance along a spiral, so I'm approximating it with circles, in hopes that it will still be useful.
My solution relies on the InterX function from FEX, to find the intersection of circles with the spiral. I am providing an animation so it is easier to understand.
The code (tested on R2017a):
function [x,y,xi,yi] = q44916610(doPlot)
%% Input handling:
if nargin < 1 || isempty(doPlot)
doPlot = false;
end
%% Initialization:
origin = [50,50];
vertspacing = 10;
thetamax = 5*(2*pi);
%% Calculation of (x,y) - underlying archimedean spiral.
b = vertspacing/(2*pi);
theta = 0:0.01:thetamax;
x = b*theta.*cos(theta) + origin(1);
y = b*theta.*sin(theta) + origin(2);
%% Calculation of equidistant (xi,yi) points on spiral.
DST = 5; cRes = 360;
numPts = ceil(vertspacing*thetamax); % Preallocation
[xi,yi] = deal(NaN(numPts,1));
if doPlot && isHG2() % Plots are only enabled if the MATLAB version is new enough.
figure(); plot(x,y,'b-'); hold on; axis equal; grid on; grid minor;
hAx = gca; hAx.XLim = [-5 105]; hAx.YLim = [-5 105];
hP = plot(xi,yi,'r*');
else
hP = struct('XData',xi,'YData',yi);
end
hP.XData(1) = origin(1); hP.YData(1) = origin(2);
for ind = 2:numPts
P = InterX([x;y], makeCircle([hP.XData(ind-1),hP.YData(ind-1)],DST/2,cRes));
[~,I] = max(abs(P(1,:)-origin(1)+1i*(P(2,:)-origin(2))));
if doPlot, pause(0.1); end
hP.XData(ind) = P(1,I); hP.YData(ind) = P(2,I);
if doPlot, pause(0.1); delete(hAx.Children(1)); end
end
xi = hP.XData(~isnan(hP.XData)); yi = hP.YData(~isnan(hP.YData));
%% Nested function(s):
function [XY] = makeCircle(cnt, R, nPts)
P = (cnt(1)+1i*cnt(2))+R*exp(linspace(0,1,nPts)*pi*2i);
if doPlot, plot(P,'Color',lines(1)); end
XY = [real(P); imag(P)];
end
end
%% Local function(s):
function tf = isHG2()
try
tf = ~verLessThan('MATLAB', '8.4');
catch
tf = false;
end
end
function P = InterX(L1,varargin)
% DOCUMENTATION REMOVED. For a full version go to:
% https://www.mathworks.com/matlabcentral/fileexchange/22441-curve-intersections
narginchk(1,2);
if nargin == 1
L2 = L1; hF = #lt; %...Avoid the inclusion of common points
else
L2 = varargin{1}; hF = #le;
end
%...Preliminary stuff
x1 = L1(1,:)'; x2 = L2(1,:);
y1 = L1(2,:)'; y2 = L2(2,:);
dx1 = diff(x1); dy1 = diff(y1);
dx2 = diff(x2); dy2 = diff(y2);
%...Determine 'signed distances'
S1 = dx1.*y1(1:end-1) - dy1.*x1(1:end-1);
S2 = dx2.*y2(1:end-1) - dy2.*x2(1:end-1);
C1 = feval(hF,D(bsxfun(#times,dx1,y2)-bsxfun(#times,dy1,x2),S1),0);
C2 = feval(hF,D((bsxfun(#times,y1,dx2)-bsxfun(#times,x1,dy2))',S2'),0)';
%...Obtain the segments where an intersection is expected
[i,j] = find(C1 & C2);
if isempty(i), P = zeros(2,0); return; end
%...Transpose and prepare for output
i=i'; dx2=dx2'; dy2=dy2'; S2 = S2';
L = dy2(j).*dx1(i) - dy1(i).*dx2(j);
i = i(L~=0); j=j(L~=0); L=L(L~=0); %...Avoid divisions by 0
%...Solve system of eqs to get the common points
P = unique([dx2(j).*S1(i) - dx1(i).*S2(j), ...
dy2(j).*S1(i) - dy1(i).*S2(j)]./[L L],'rows')';
function u = D(x,y)
u = bsxfun(#minus,x(:,1:end-1),y).*bsxfun(#minus,x(:,2:end),y);
end
end
Result:
Note that in the animation above, the diameter of the circle (and hence the distance between the red points) is 10 and not 5.
I have a 3D volume and a 2D image and an approximate mapping (affine transformation with no skwewing, known scaling, rotation and translation approximately known and need fitting) between the two. Because there is an error in this mapping and I would like to further register the 2D image to the 3D volume. I have not written code for registration purposes before, but because I can't find any programs or code to solve this I would like to try and do this. I believe the standard for registration is to optimize mutual information. I think this would also be suitable here, because the intensities are not equal between the two images. So I think I should make a function for the transformation, a function for the mutual information and a function for optimization.
I did find some Matlab code on a mathworks thread from two years ago, based on an article. The OP reports that she managed to get the code to work, but I'm not getting how she did that exactly. Also in the IP package for matlab there is an implementation, but I dont have that package and there does not seem to be an equivalent for octave. SPM is a program that uses matlab and has registration implemented, but does not cope with 2d to 3d registration. On the file exchange there is a brute force method that registers two 2D images using mutual information.
What she does is pass a multi planar reconstruction function and an similarity/error function into a minimization algorithm. But the details I don't quite understand. Maybe it would be better to start fresh:
load mri; volume = squeeze(D);
phi = 3; theta = 2; psi = 5; %some small angles
tx = 1; ty = 1; tz = 1; % some small translation
dx = 0.25, dy = 0.25, dz = 2; %different scales
t = [tx; ty; tz];
r = [phi, theta, psi]; r = r*(pi/180);
dims = size(volume);
p0 = [round(dims(1)/2);round(dims(2)/2);round(dims(3)/2)]; %image center
S = eye(4); S(1,1) = dx; S(2,2) = dy; S(3,3) = dz;
Rx=[1 0 0 0;
0 cos(r(1)) sin(r(1)) 0;
0 -sin(r(1)) cos(r(1)) 0;
0 0 0 1];
Ry=[cos(r(2)) 0 -sin(r(2)) 0;
0 1 0 0;
sin(r(2)) 0 cos(r(2)) 0;
0 0 0 1];
Rz=[cos(r(3)) sin(r(3)) 0 0;
-sin(r(3)) cos(r(3)) 0 0;
0 0 1 0;
0 0 0 1];
R = S*Rz*Ry*Rx;
%make affine matrix to rotate about center of image
T1 = ( eye(3)-R(1:3,1:3) ) * p0(1:3);
T = T1 + t; %add translation
A = R;
A(1:3,4) = T;
Rold2new = A;
Rnew2old = inv(Rold2new);
%the transformation
[xx yy zz] = meshgrid(1:dims(1),1:dims(2),1:1);
coordinates_axes_new = [xx(:)';yy(:)';zz(:)'; ones(size(zz(:)))'];
coordinates_axes_old = Rnew2old*coordinates_axes_new;
Xcoordinates = reshape(coordinates_axes_old(1,:), dims(1), dims(2), dims(3));
Ycoordinates = reshape(coordinates_axes_old(2,:), dims(1), dims(2), dims(3));
Zcoordinates = reshape(coordinates_axes_old(3,:), dims(1), dims(2), dims(3));
%interpolation/reslicing
method = 'cubic';
slice= interp3(volume, Xcoordinates, Ycoordinates, Zcoordinates, method);
%so now I have my slice for which I would like to find the correct position
% first guess for A
A0 = eye(4); A0(1:3,4) = T1; A0(1,1) = dx; A0(2,2) = dy; A0(3,3) = dz;
% this is pretty close to A
% now how would I fit the slice to the volume by changing A0 and examining some similarity measure?
% probably maximize mutual information?
% http://www.mathworks.com/matlabcentral/fileexchange/14888-mutual-information-computation/content//mi/mutualinfo.m
Ok I was hoping for someone else's approach, that would probably have been better than mine as I have never done any optimization or registration before. So I waited for Knedlsepps bounty to almost finish. But I do have some code thats working now. It will find a local optimum so the initial guess must be good. I wrote some functions myself, took some functions from the file exchange as is and I extensively edited some other functions from the file exchange. Now that I put all the code together to work as an example here, the rotations are off, will try and correct that. Im not sure where the difference in code is between the example and my original code, must have made a typo in replacing some variables and data loading scheme.
What I do is I take the starting affine transformation matrix, decompose it to an orthogonal matrix and an upper triangular matrix. I then assume the orthogonal matrix is my rotation matrix so I calculate the euler angles from that. I directly take the translation from the affine matrix and as stated in the problem I assume I know the scaling matrix and there is no shearing. So then I have all degrees of freedom for the affine transformation, which my optimisation function changes and constructs a new affine matrix from, applies it to the volume and calculates the mutual information. The matlab optimisation function patternsearch then minimises 1-MI/MI_max.
What I noticed when using it on my real data which are multimodal brain images is that it works much better on brain extracted images, so with the skull and tissue outside of the skull removed.
%data
load mri; volume = double(squeeze(D));
%transformation parameters
phi = 3; theta = 1; psi = 5; %some small angles
tx = 1; ty = 1; tz = 3; % some small translation
dx = 0.25; dy = 0.25; dz = 2; %different scales
t = [tx; ty; tz];
r = [phi, theta, psi]; r = r*(pi/180);
%image center and size
dims = size(volume);
p0 = [round(dims(1)/2);round(dims(2)/2);round(dims(3)/2)];
%slice coordinate ranges
range_x = 1:dims(1)/dx;
range_y = 1:dims(2)/dy;
range_z = 1;
%rotation
R = dofaffine([0;0;0], r, [1,1,1]);
T1 = ( eye(3)-R(1:3,1:3) ) * p0(1:3); %rotate about p0
%scaling
S = eye(4); S(1,1) = dx; S(2,2) = dy; S(3,3) = dz;
%translation
T = [[eye(3), T1 + t]; [0 0 0 1]];
%affine
A = T*R*S;
% first guess for A
r00 = [1,1,1]*pi/180;
R00 = dofaffine([0;0;0], r00, [1 1 1]);
t00 = T1 + t + ( eye(3) - R00(1:3,1:3) ) * p0;
A0 = dofaffine( t00, r00, [dx, dy, dz] );
[ t0, r0, s0 ] = dofaffine( A0 );
x0 = [ t0.', r0, s0 ];
%the transformation
slice = affine3d(volume, A, range_x, range_y, range_z, 'cubic');
guess = affine3d(volume, A0, range_x, range_y, range_z, 'cubic');
%initialisation
Dt = [1; 1; 1];
Dr = [2 2 2].*pi/180;
Ds = [0 0 0];
Dx = [Dt', Dr, Ds];
%limits
LB = x0-Dx;
UB = x0+Dx;
%other inputs
ref_levels = length(unique(slice));
Qref = imquantize(slice, ref_levels);
MI_max = MI_GG(Qref, Qref);
%patternsearch options
options = psoptimset('InitialMeshSize',0.03,'MaxIter',20,'TolCon',1e-5,'TolMesh',5e-5,'TolX',1e-6,'PlotFcns',{#psplotbestf,#psplotbestx});
%optimise
[x2, MI_norm_neg, exitflag_len] = patternsearch(#(x) AffRegOptFunc(x, slice, volume, MI_max, x0), x0,[],[],[],[],LB(:),UB(:),options);
%check
p0 = [round(size(volume)/2).'];
R0 = dofaffine([0;0;0], x2(4:6)-x0(4:6), [1 1 1]);
t1 = ( eye(3) - R0(1:3,1:3) ) * p0;
A2 = dofaffine( x2(1:3).'+t1, x2(4:6), x2(7:9) ) ;
fitted = affine3d(volume, A2, range_x, range_y, range_z, 'cubic');
overlay1 = imfuse(slice, guess);
overlay2 = imfuse(slice, fitted);
figure(101);
ax(1) = subplot(1,2,1); imshow(overlay1, []); title('pre-reg')
ax(2) = subplot(1,2,2); imshow(overlay2, []); title('post-reg');
linkaxes(ax);
function normed_score = AffRegOptFunc( x, ref_im, reg_im, MI_max, x0 )
t = x(1:3).';
r = x(4:6);
s = x(7:9);
rangx = 1:size(ref_im,1);
rangy = 1:size(ref_im,2);
rangz = 1:size(ref_im,3);
ref_levels = length(unique(ref_im));
reg_levels = length(unique(reg_im));
t0 = x0(1:3).';
r0 = x0(4:6);
s0 = x0(7:9);
p0 = [round(size(reg_im)/2).'];
R = dofaffine([0;0;0], r-r0, [1 1 1]);
t1 = ( eye(3) - R(1:3,1:3) ) * p0;
t = t + t1;
Ap = dofaffine( t, r, s );
reg_im_t = affine3d(reg_im, A, rangx, rangy, rangz, 'cubic');
Qref = imquantize(ref_im, ref_levels);
Qreg = imquantize(reg_im_t, reg_levels);
MI = MI_GG(Qref, Qreg);
normed_score = 1-MI/MI_max;
end
function [ varargout ] = dofaffine( varargin )
% [ t, r, s ] = dofaffine( A )
% [ A ] = dofaffine( t, r, s )
if nargin == 1
%affine to degrees of freedom (no shear)
A = varargin{1};
[T, R, S] = decompaffine(A);
r = GetEulerAngles(R(1:3,1:3));
s = [S(1,1), S(2,2), S(3,3)];
t = T(1:3,4);
varargout{1} = t;
varargout{2} = r;
varargout{3} = s;
elseif nargin == 3
%degrees of freedom to affine (no shear)
t = varargin{1};
r = varargin{2};
s = varargin{3};
R = GetEulerAngles(r); R(4,4) = 1;
S(1,1) = s(1); S(2,2) = s(2); S(3,3) = s(3); S(4,4) = 1;
T = eye(4); T(1,4) = t(1); T(2,4) = t(2); T(3,4) = t(3);
A = T*R*S;
varargout{1} = A;
else
error('incorrect number of input arguments');
end
end
function [ T, R, S ] = decompaffine( A )
%I assume A = T * R * S
T = eye(4);
R = eye(4);
S = eye(4);
%decompose in orthogonal matrix q and upper triangular matrix r
%I assume q is a rotation matrix and r is a scale and shear matrix
%matlab 2014 can force real solution
[q r] = qr(A(1:3,1:3));
R(1:3,1:3) = q;
S(1:3,1:3) = r;
% A*S^-1*R^-1 = T*R*S*S^-1*R^-1 = T*R*I*R^-1 = T*R*R^-1 = T*I = T
T = A*inv(S)*inv(R);
t = T(1:3,4);
T = [eye(4) + [[0 0 0;0 0 0;0 0 0;0 0 0],[t;0]]];
end
function [varargout]= GetEulerAngles(R)
assert(length(R)==3)
dims = size(R);
if min(dims)==1
rx = R(1); ry = R(2); rz = R(3);
R = [[ cos(ry)*cos(rz), -cos(ry)*sin(rz), sin(ry)];...
[ cos(rx)*sin(rz) + cos(rz)*sin(rx)*sin(ry), cos(rx)*cos(rz) - sin(rx)*sin(ry)*sin(rz), -cos(ry)*sin(rx)];...
[ sin(rx)*sin(rz) - cos(rx)*cos(rz)*sin(ry), cos(rz)*sin(rx) + cos(rx)*sin(ry)*sin(rz), cos(rx)*cos(ry)]];
varargout{1} = R;
else
ry=asin(R(1,3));
rz=acos(R(1,1)/cos(ry));
rx=acos(R(3,3)/cos(ry));
if nargout > 1 && nargout < 4
varargout{1} = rx;
varargout{2} = ry;
varargout{3} = rz;
elseif nargout == 1
varargout{1} = [rx ry rz];
else
error('wrong number of output arguments');
end
end
end
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: