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

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.

Answer 1

[Jonathan]

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))]] );

Answer 2

[Cristian Duguet]

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.