source: trunk/src/toolbox_calib/rigid_motion.m @ 935

Last change on this file since 935 was 926, checked in by sommeria, 8 years ago

geometry cqlib updated

File size: 1.6 KB
Line 
1function [Y,dYdom,dYdT] = rigid_motion(X,om,T);
2
3%rigid_motion.m
4%
5%[Y,dYdom,dYdT] = rigid_motion(X,om,T)
6%
7%Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om).
8%
9%INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
10%       (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
11%               om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
12%
13%OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points)
14%        dYdom: Derivative of Y with respect to om ((3N)x3 matrix)
15%        dYdT: Derivative of Y with respect to T ((3N)x3 matrix)
16%
17%Definitions:
18%Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
19%The coordinate vector of P in the camera reference frame is: Y = R*X + T
20%where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
21%
22%Important function called within that program:
23%
24%rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
25
26
27
28if nargin < 3,
29   T = zeros(3,1);
30   if nargin < 2,
31      om = zeros(3,1);
32      if nargin < 1,
33         error('Need at least a 3D structure as input (in rigid_motion.m)');
34         return;
35      end;
36   end;
37end;
38
39
40[R,dRdom] = rodrigues(om);
41
42[m,n] = size(X);
43
44Y = R*X + repmat(T,[1 n]);
45
46if nargout > 1,
47   
48
49dYdR = zeros(3*n,9);
50dYdT = zeros(3*n,3);
51
52dYdR(1:3:end,1:3:end) =  X';
53dYdR(2:3:end,2:3:end) =  X';
54dYdR(3:3:end,3:3:end) =  X';
55
56dYdT(1:3:end,1) =  ones(n,1);
57dYdT(2:3:end,2) =  ones(n,1);
58dYdT(3:3:end,3) =  ones(n,1);
59
60dYdom = dYdR * dRdom;
61
62end;
63
64
65
66
Note: See TracBrowser for help on using the repository browser.