# Non-linear Optimization for adjusting one trajectory to another (one matrix transformed to match other).

1 view (last 30 days)
Cristian Duguet on 15 Nov 2011
Hi!
in the M-code of my GUI Program, I have to adjust one set of the XYZ points (a trajectory) to another using rotation matrices. So, let's say each trajectory Matrix T1 and T2 has a size [100x3].
Initializing the same starting point for T1 and T2, so that both begin in [0 0 0] (T1 = T1 - repmat(T1(1,:),100,1); and the same for T2), I want to rotate the T2 trajectory to fit T1 in the order ZXY, using the rotation Matrices Mx, My, Mz, which I define using the variables rx, ry and rz, and also scaling the trajectory with c.
The thing is then, that I want to minimize:
norm(T1 - c*T2*Mz*Mx*My)
Where :
Mx = [ [1 0 0] ; [0 cos(rx) sin(rx) ] ; [0 -sin(rx) cos(rx)]];
My = [ [cos(ry) 0 -sin(ry)] ; [0 1 0] ; [sin(ry) 0 cos(ry)]];
Mz = [ [cos(rz) sin(rz) 0] ; [-sin(rz) cos(rz) 0] ; [0 0 1]];
My attempts so far:
1.- I tried defining an inline function, putting the rotation matrices totally expanded for being directly depending of my variables of interest rx,ry,rz,c as a vector x of size 1x4.
difference_fun=inline(strcat('norm( T1 - x(4)*T2 * ',...
'[ [cos(x(3)) sin(x(3)) 0] ; [-sin(x(3)) cos(x(3)) 0] ; [0 0 1]] * ', ...
'[ [1 0 0] ; [0 cos(x(1)) sin(x(1)) ] ; [0 -sin(x(1)) cos(x(1))]] * ', ...
'[ [cos(x(2)) 0 -sin(x(2))] ; [0 1 0] ; [sin(x(2)) 0 cos(x(2))]] )'), ...
'x');
x0 = [0,0,0,1]; % Starting guess
options = optimset('LargeScale','off');
[x,fval,exitflag,output] = fminunc(difference_fun,x0,options)
The problem is, that inline functions don't take variables as parameters, it just says that it doesn't find function or variable "T1".
2.- My second attempt was to define a separate function inside my GUI M-file, in which I could pass rx,ry,rz,c as input argument and also "handles" as input arguments (handles contains handles.t1 and handles.t2). With this, I still can't figure out how to use an Optimizing tool to optimize along all my arguments and letting 'handles' fixed. (Also, I am now thinking that passing handles as an to an unimportant function like that is not a clever idea).
How could I get the right rx,ry,rz and c that adjust the trajectory of T2 to T1 ?
Thank you very much.

Jonathan on 15 Nov 2011
Try an anonymous function handle instead of an inline function, like this.
difference_fun = @(x) norm( T1 - x(4)*T2 * ...
[[cos(x(3)) sin(x(3)) 0]; [-sin(x(3)) cos(x(3)) 0]; [0 0 1]] * ...
[[1 0 0]; [0 cos(x(1)) sin(x(1))]; [0 -sin(x(1)) cos(x(1))]] * ...
[[cos(x(2)) 0 -sin(x(2))]; [0 1 0]; [sin(x(2)) 0 cos(x(2))]] );

Cristian Duguet on 17 Nov 2011
Thank you very much! Exactly was I was looking for, and found also in http://www.mathworks.de/help/toolbox/optim/ug/brhkghv-7.html Anyways from the three optios I think the anonymous function is the one that best fits to me.