1 | function [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 | |
---|

28 | if 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; |
---|

37 | end; |
---|

38 | |
---|

39 | |
---|

40 | [R,dRdom] = rodrigues(om); |
---|

41 | |
---|

42 | [m,n] = size(X); |
---|

43 | |
---|

44 | Y = R*X + repmat(T,[1 n]); |
---|

45 | |
---|

46 | if nargout > 1, |
---|

47 | |
---|

48 | |
---|

49 | dYdR = zeros(3*n,9); |
---|

50 | dYdT = zeros(3*n,3); |
---|

51 | |
---|

52 | dYdR(1:3:end,1:3:end) = X'; |
---|

53 | dYdR(2:3:end,2:3:end) = X'; |
---|

54 | dYdR(3:3:end,3:3:end) = X'; |
---|

55 | |
---|

56 | dYdT(1:3:end,1) = ones(n,1); |
---|

57 | dYdT(2:3:end,2) = ones(n,1); |
---|

58 | dYdT(3:3:end,3) = ones(n,1); |
---|

59 | |
---|

60 | dYdom = dYdR * dRdom; |
---|

61 | |
---|

62 | end; |
---|

63 | |
---|

64 | |
---|

65 | |
---|

66 | |
---|