Matlab solution for non-homogenous heat equation using finite differences - matlab

Given the following PDE (non-homogenous heat equation):
ut(x,t) = c2uxx(x,t) + f(x,t)
u(0,t) = u(l,t) = 0
u(x,0) = g(x)
0 < x < l ; t > 0 ; c > 0
I wrote the following code in Matlab, to solve the problem using finite differences:
syms xj tk
% Manually define this values
c = 9;
f(xj,tk) = xj;
g(xj) = 0*xj;
l = 1;
Tmax = 0.1;
% Grid definition
Nx = 50;
Nt = 50;
hx = 1/Nx;
ht = 1/Nt;
x = 0:hx:l;
t = 0:ht:Tmax;
lambda = c^2*ht/hx^2;
% Our target
u = zeros(Nx+1,Nt+1);
% Initial values
for j=1:Nx,
u(j,1) = g(x(j)); % u(x,0) = g(x)
end
for k=1:Nx,
u(1,k+1) = 0; % border condition u(0,t) = 0
for j=2:Nt,
u(j,k+1) = u(j,k) + lambda*(u(j+1,k)-2*u(j,k)+u(j-1,k)) + ht*f(j,k); % the formula here is ok
end
u(Nt,k+1) = 0; % border condition u(l,t) = 0
end
contour3(u)
For some reason that I cant't figure out, data is only appearing in the last columns and in a very strange way.
I'm guessing the implementation of the BC's are doing something nasty. But I don't see it.
Is there something that I'm missing?
Thanks in advance!

Related

Matlab Error: Index in Position 1 exceeds array bounds

I am attempting to create a gamma distribution in MATLAB; however, I keep receiving the error:
Index in Position 1 exceeds array bounds (must not exceed 100).
Assuming I am reading this correctly, it is referring to variable M that is simply = 2500 (the number of pseudo-random variables I am using for this project).
I was hoping someone can explain what is wrong with my logic and possibly a solution.
alpha = 0.5;
w = gamma_rdn(M,alpha);
x1 = (0.0001:0.001:1); % For plot
figure(5)
subplot(2,1,1);hist(w);title('Histogram of Gamma RDN');
subplot(2,1,2);plot(x1,pdf('gam',x1,alpha,1));title('Theoretical Gamma Density with \alpha = 0.5');
axis([0 1 0 100]);
% The gamma_rdn function is implemented as follows:
function[w] = gamma_rdn(M,alpha)
% Generate random numbers from the gamma distribution with parameter
% alpha <= 1, beta = 1
pe = exp(1);
w = zeros(M,1);
u = rand(100,1);
b = (alpha + pe)/pe;
i = 0;
j = 0;
while j < M
i = i+1;
y = b*u(i,1);
if y <= 1
z = y^(1/alpha);
i = i+1;
if u(i,1) <= exp(-z)
j = j+1;
w(j,1) = z;
else
i = i+1;
end
else
z = -log((b-y)/alpha);
i = i+1;
if u(i,1) <= z^(alpha - 1)
j = j+1;
w(j,1) = z;
else
i = i+1;
end
end
end
if i > 95
u = rand(100,1);
i = 0;
end
end
Is there a particular reason you chose u = rand(100,1)?
The problem is coming because in while loop, as soon as variable i exceeds 100 (say i=101), y = b*u(i,1) becomes invalid. That is, you are trying to access u(101,1) while the size of u is (100,1).
If there's not particular reason, try a large enough size, like, u = rand(10000,1).

Finding correct index value for matrix in Matlab using meshgrid

I'm trying to build make a code where an equation is not calculated for some certain values. I have a meshgrid with several values for x and y and I want to include a for loop that will calculate some values for most of the points in the meshgrid but I'm trying to include in that loop a condition that if the points have a specified index, the value will not be calculated. In my second group of for/if loops, I want to say that for all values of i and k (row and column), the value for z and phi are calculated with the exception of the specified i and k values (in the if loop). What I'm doing at the moment is not working...
The error I'm getting is:
The expression to the left of the equals sign is not a valid target for an assignment.
Here is my code at the moment. I'd really appreciate any advice on this! Thanks in advance
U_i = 20;
a = 4;
c = -a*5;
b = a*10;
d = -20;
e = 20;
n = a*10;
[x,y] = meshgrid([c:(b-c)/n:b],[d:(e-d)/n:e]');
for i = 1:length(x)
for k = 1:length(x)
% Zeroing values where cylinder is
if sqrt(x(i,k).^2 + y(i,k).^2) < a
x(i,k) = 0;
y(i,k) = 0;
end
end
end
r = sqrt(x.^2 + y.^2);
theta = atan2(y,x);
z = zeros(length(x));
phi = zeros(length(x));
for i = 1:length(x)
for k = 1:length(x)
if (i > 16 && i < 24 && k > 16 && k <= length(x))
z = 0;
phi = 0;
else
z = U_i.*r.*(1-a^2./r.^2).*sin(theta); % Stream function
phi = U_i*r.*(1+a^2./r.^2).*cos(theta); % Velocity potential
end
end
end
The original code in the question can be rewritten as seen below. Pay attention in the line with ind(17:24,:) since your edit now excludes 24 and you original question included 24.
U_i = 20;
a = 4;
c = -a*5;
b = a*10;
d = -20;
e = 20;
n = a*10;
[x,y] = meshgrid([c:(b-c)/n:b],[d:(e-d)/n:e]');
ind = find(sqrt(x.^2 + y.^2) < a);
x(ind) = 0;
y(ind) = 0;
r = sqrt(x.^2 + y.^2);
theta = atan2(y,x);
ind = true(size(x));
ind(17:24,17:length(x)) = false;
z = zeros(size(x));
phi = zeros(size(x));
z(ind) = U_i.*r(ind).*(1-a^2./r(ind).^2).*sin(theta(ind)); % Stream function
phi(ind) = U_i.*r(ind).*(1+a^2./r(ind).^2).*cos(theta(ind)); % Velocity potential

Index exceeds matrix dimensions error in Runge-Kutta method: Matlab

I'm trying to make a time stepping code using the 4th order Runge-Kutta method but am running into issues indexing one of my values properly. My code is:
clc;
clear all;
L = 32; M = 32; N = 32; % No. of elements
Lx = 2; Ly = 2; Lz = 2; % Size of each element
dx = Lx/L; dy = Ly/M; dz = Lz/N; % Step size
Tt = 1;
t0 = 0; % Initial condition
T = 50; % Final time
dt = (Tt-t0)/T; % Determining time step interval
% Wave characteristics
H = 2; % Wave height
a = H/2; % Amplitude
Te = 6; % Period
omega = 2*pi/Te; % Wave rotational frequency
d = 25; % Water depth
x = 0; % Location of cylinder axis
u0(1:L,1:M,1:N,1) = 0; % Setting up solution space matrix (u values)
v0(1:L,1:M,1:N,1) = 0; % Setting up solution space matrix (v values)
w0(1:L,1:M,1:N,1) = 0; % Setting up solution space matrix (w values)
[k,L] = disp(d,omega); % Solving for k and wavelength using Newton-Raphson function
%u = zeros(1,50);
%v = zeros(1,50);
%w = zeros(1,50);
time = 1:1:50;
for t = 1:T
for i = 1:L
for j = 1:M
for k = 1:N
eta(i,j,k,t) = a*cos(omega*time(1,t);
u(i,j,k,1) = u0(i,j,k,1);
v(i,j,k,1) = v0(i,j,k,1);
w(i,j,k,1) = w0(i,j,k,1);
umag(i,j,k,t) = a*omega*(cosh(k*(d+eta(i,j,k,t))))/sinh(k*d);
vmag(i,j,k,t) = 0;
wmag(i,j,k,t) = -a*omega*(sinh(k*(d+eta(i,j,k,t))))/sinh(k*d);
uRHS(i,j,k,t) = umag(i,j,k,t)*cos(k*x-omega*t);
vRHS(i,j,k,t) = vmag(i,j,k,t)*sin(k*x-omega*t);
wRHS(i,j,k,t) = wmag(i,j,k,t)*sin(k*x-omega*t);
k1x(i,j,k,t) = dt*uRHS(i,j,k,t);
k2x(i,j,k,t) = dt*(0.5*k1x(i,j,k,t) + dt*uRHS(i,j,k,t));
k3x(i,j,k,t) = dt*(0.5*k2x(i,j,k,t) + dt*uRHS(i,j,k,t));
k4x(i,j,k,t) = dt*(k3x(i,j,k,t) + dt*uRHS(i,j,k,t));
u(i,j,k,t+1) = u(i,j,k,t) + (1/6)*(k1x(i,j,k,t) + 2*k2x(i,j,k,t) + 2*k3x(i,j,k,t) + k4x(i,j,k,t));
k1y(i,j,k,t) = dt*vRHS(i,j,k,t);
k2y(i,j,k,t) = dt*(0.5*k1y(i,j,k,t) + dt*vRHS(i,j,k,t));
k3y(i,j,k,t) = dt*(0.5*k2y(i,j,k,t) + dt*vRHS(i,j,k,t));
k4y(i,j,k,t) = dt*(k3y(i,j,k,t) + dt*vRHS(i,j,k,t));
v(i,j,k,t+1) = v(i,j,k,t) + (1/6)*(k1y(i,j,k,t) + 2*k2y(i,j,k,t) + 2*k3y(i,j,k,t) + k4y(i,j,k,t));
k1z(i,j,k,t) = dt*wRHS(i,j,k,t);
k2z(i,j,k,t) = dt*(0.5*k1z(i,j,k,t) + dt*wRHS(i,j,k,t));
k3z(i,j,k,t) = dt*(0.5*k2z(i,j,k,t) + dt*wRHS(i,j,k,t));
k4z(i,j,k,t) = dt*(k3z(i,j,k,t) + dt*wRHS(i,j,k,t));
w(i,j,k,t+1) = w(i,j,k,t) + (1/6)*(k1z(i,j,k,t) + 2*k2z(i,j,k,t) + 2*k3z(i,j,k,t) + k4z(i,j,k,t));
a(i,j,k,t+1) = ((u(i,j,k,t+1))^2 + (v(i,j,k,t+1))^2 + (w(i,j,k,t+1))^2)^0.5;
end
end
end
end
At the moment, the values seem to be fine for the first iteration but then I have the error Index exceeds matrix dimension in the line calculating eta. I understand that I am not correctly indexing the eta value but am not sure how to correct this.
My goal is to update the value of eta for each loop of t and then use that new eta value for the rest of the calculations.
I'm still quite new to programming and am trying to understand indexing, especially in 3 or 4 dimensional matrices and would really appreciate any advice in correctly calculating this value.
Thanks in advance for any advice!
You declare
time = 1:1:50;
which is just a row vector but access it here
eta(i,j,k,t) = a*cos(omega*time(i,j,k,t));
as if it were an array with 4 dimensions.
To correctly access element x of time you need to use syntax
time(1,x);
(as it is a 1 x 50 array)

Frank - Wolfe Algorithm in matlab

I'm trying to solve the following question :
maximize x^2-5x+y^2-3y
x+y <= 8
x<=2
x,y>= 0
By using Frank Wolf algorithm ( according to http://web.mit.edu/15.053/www/AMP-Chapter-13.pdf ).
But after running of the following program:
syms x y t;
f = x^2-5*x+y^2-3*y;
fdx = diff(f,1,x); % f'x
fdy = diff(f,1,y); % y'x
x0 = [0 0]; %initial point
A = [1 1;1 0]; %constrains matrix
b = [8;2];
lb = zeros(1,2);
eps = 0.00001;
i = 1;
X = [inf inf];
Z = zeros(2,200); %result for end points (x1,x2)
rr = zeros(1,200);
options = optimset('Display','none');
while( all(abs(X-x0)>[eps,eps]) && i < 200)
%f'x(x0)
c1 = subs(fdx,x,x0(1));
c1 = subs(c1,y,x0(2));
%f'y(x0)
c2 = subs(fdy,x,x0(1));
c2 = subs(c2,y,x0(2));
%optimization point of linear taylor function
ys = linprog((-[c1;c2]),A,b,[],[],lb,[],[],options);
%parametric representation of line
xt = (1-t)*x0(1)+t*ys(1,1);
yt = (1-t)*x0(2)+t*ys(2,1);
%f(x=xt,y=yt)
ft = subs(f,x,xt);
ft = subs(ft,y,yt);
%f't(x=xt,y=yt)
ftd = diff(ft,t,1);
%f't(x=xt,y=yt)=0 -> for max point
[t1] = solve(ftd); % (t==theta)
X = double(x0);%%%%%%%%%%%%%%%%%
% [ xt(t=t1) yt(t=t1)]
xnext(1) = subs(xt,t,t1) ;
xnext(2) = subs(yt,t,t1) ;
x0 = double(xnext);
Z(1,i) = x0(1);
Z(2,i) = x0(2);
i = i + 1;
end
x_point = Z(1,:);
y_point = Z(2,:);
% Draw result
scatter(x_point,y_point);
hold on;
% Print results
fprintf('The answer is:\n');
fprintf('x = %.3f \n',x0(1));
fprintf('y = %.3f \n',x0(2));
res = x0(1)^2 - 5*x0(1) + x0(2)^2 - 3*x0(2);
fprintf('f(x0) = %.3f\n',res);
I get the following result:
x = 3.020
y = 0.571
f(x0) = -7.367
And this no matter how many iterations I running this program (1,50 or 200).
Even if I choose a different starting point (For example, x0=(1,6) ), I get a negative answer to most.
I know that is an approximation, but the result should be positive (for x0 final, in this case).
My question is : what's wrong with my implementation?
Thanks in advance.
i changed a few things, it still doesn't look right but hopefully this is getting you in the right direction. It looks like the intial x0 points make a difference to how the algorithm converges.
Also make sure to check what i is after running the program, to determine if it ran to completion or exceeded the maximum iterations
lb = zeros(1,2);
ub = [2,8]; %if x+y<=8 and x,y>0 than both x,y < 8
eps = 0.00001;
i_max = 100;
i = 1;
X = [inf inf];
Z = zeros(2,i_max); %result for end points (x1,x2)
rr = zeros(1,200);
options = optimset('Display','none');
while( all(abs(X-x0)>[eps,eps]) && i < i_max)
%f'x(x0)
c1 = subs(fdx,x,x0(1));
c1 = subs(c1,y,x0(2));
%f'y(x0)
c2 = subs(fdy,x,x0(1));
c2 = subs(c2,y,x0(2));
%optimization point of linear taylor function
[ys, ~ , exit_flag] = linprog((-[c1;c2]),A,b,[],[],lb,ub,x0,options);
so here is the explanation of the changes
ub, uses our upper bound. After i added a ub, the result immediately changed
x0, start this iteration from the previous point
exit_flag this allows you to check exit_flag after execution (it always seems to be 1 indicating it solved the problem correctly)

Rigidly register a 2D image to a 3D volume with good initial guess for affine transformation

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