g_computemotion.m 1.59 KB
Newer Older
Daniel Bourgault's avatar
Daniel Bourgault committed
1
function[ A, T ] = g_computemotion( fx, fy, ft, roi )
2

Daniel Bourgault's avatar
Daniel Bourgault committed
3
4
5
6
[ydim,xdim] = size(fx);
[x,y] = meshgrid( [1:xdim]-xdim/2, [1:ydim]-ydim/2 );

%%% TRIM EDGES
7
8
9
fx  = fx( 3:end-2, 3:end-2 );
fy  = fy( 3:end-2, 3:end-2 );
ft  = ft( 3:end-2, 3:end-2 );
Daniel Bourgault's avatar
Daniel Bourgault committed
10
roi = roi( 3:end-2, 3:end-2 );
11
12
x   = x( 3:end-2, 3:end-2 );
y   = y( 3:end-2, 3:end-2 );
Daniel Bourgault's avatar
Daniel Bourgault committed
13
14

ind = find( roi > 0 );
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
x   = x(ind); 
y   = y(ind);
fx  = fx(ind); 
fy  = fy(ind); 
ft  = ft(ind);

xfx = x.*fx; 
xfy = x.*fy; 
yfx = y.*fx; 
yfy = y.*fy;

M(1,1) = sum( xfx .* xfx ); 
M(1,2) = sum( xfx .* yfx ); 
M(1,3) = sum( xfx .* xfy );
M(1,4) = sum( xfx .* yfy ); 
M(1,5) = sum( xfx .* fx ); 
M(1,6) = sum( xfx .* fy );
M(2,1) = M(1,2); 
M(2,2) = sum( yfx .* yfx ); 
M(2,3) = sum( yfx .* xfy );
M(2,4) = sum( yfx .* yfy ); 
M(2,5) = sum( yfx .* fx ); 
M(2,6) = sum( yfx .* fy );
M(3,1) = M(1,3); 
M(3,2) = M(2,3); 
M(3,3) = sum( xfy .* xfy );
M(3,4) = sum( xfy .* yfy ); 
M(3,5) = sum( xfy .* fx ); 
M(3,6) = sum( xfy .* fy );
M(4,1) = M(1,4); 
M(4,2) = M(2,4); 
M(4,3) = M(3,4);
M(4,4) = sum( yfy .* yfy ); 
M(4,5) = sum( yfy .* fx ); 
M(4,6) = sum( yfy .* fy );
M(5,1) = M(1,5); 
M(5,2) = M(2,5); 
M(5,3) = M(3,5);
M(5,4) = M(4,5); 
M(5,5) = sum( fx .* fx ); 
M(5,6) = sum( fx .* fy );
M(6,1) = M(1,6); 
M(6,2) = M(2,6); 
M(6,3) = M(3,6);
M(6,4) = M(4,6); 
M(6,5) = M(5,6); 
M(6,6) = sum( fy .* fy );
Daniel Bourgault's avatar
Daniel Bourgault committed
62
63
64

k = ft + xfx + yfy;

65
66
67
68
69
70
b(1) = sum( k .* xfx ); 
b(2) = sum( k .* yfx );
b(3) = sum( k .* xfy ); 
b(4) = sum( k .* yfy );
b(5) = sum( k .* fx ); 
b(6) = sum( k .* fy );
Daniel Bourgault's avatar
Daniel Bourgault committed
71
72
73
74

%v = inv(M) * b';
v = M\b';

75
%A = [1 0 ; 0 1]; % For translation only 
Daniel Bourgault's avatar
Daniel Bourgault committed
76
A = [v(1) v(2) ; v(3) v(4)];
77
T = [v(5) ; v(6)];