1 | %=======================================================================
|
---|
2 | % Copyright 2008-2014, LEGI UMR 5519 / CNRS UJF G-INP, Grenoble, France
|
---|
3 | % http://www.legi.grenoble-inp.fr
|
---|
4 | % Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr
|
---|
5 | %
|
---|
6 | % This file is part of the toolbox UVMAT.
|
---|
7 | %
|
---|
8 | % UVMAT is free software; you can redistribute it and/or modify
|
---|
9 | % it under the terms of the GNU General Public License as published
|
---|
10 | % by the Free Software Foundation; either version 2 of the license,
|
---|
11 | % or (at your option) any later version.
|
---|
12 | %
|
---|
13 | % UVMAT is distributed in the hope that it will be useful,
|
---|
14 | % but WITHOUT ANY WARRANTY; without even the implied warranty of
|
---|
15 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
---|
16 | % GNU General Public License (see LICENSE.txt) for more details.
|
---|
17 | %=======================================================================
|
---|
18 |
|
---|
19 | function [Y,dYdom,dYdT] = rigid_motion(X,om,T);
|
---|
20 |
|
---|
21 | %rigid_motion.m
|
---|
22 | %
|
---|
23 | %[Y,dYdom,dYdT] = rigid_motion(X,om,T)
|
---|
24 | %
|
---|
25 | %Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om).
|
---|
26 | %
|
---|
27 | %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)
|
---|
28 | % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame
|
---|
29 | % om: rotation vector (3x1 vector); T: translation vector (3x1 vector)
|
---|
30 | %
|
---|
31 | %OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points)
|
---|
32 | % dYdom: Derivative of Y with respect to om ((3N)x3 matrix)
|
---|
33 | % dYdT: Derivative of Y with respect to T ((3N)x3 matrix)
|
---|
34 | %
|
---|
35 | %Definitions:
|
---|
36 | %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)
|
---|
37 | %The coordinate vector of P in the camera reference frame is: Y = R*X + T
|
---|
38 | %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);
|
---|
39 | %
|
---|
40 | %Important function called within that program:
|
---|
41 | %
|
---|
42 | %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector
|
---|
43 |
|
---|
44 |
|
---|
45 |
|
---|
46 | if nargin < 3,
|
---|
47 | T = zeros(3,1);
|
---|
48 | if nargin < 2,
|
---|
49 | om = zeros(3,1);
|
---|
50 | if nargin < 1,
|
---|
51 | error('Need at least a 3D structure as input (in rigid_motion.m)');
|
---|
52 | return;
|
---|
53 | end;
|
---|
54 | end;
|
---|
55 | end;
|
---|
56 |
|
---|
57 |
|
---|
58 | [R,dRdom] = rodrigues(om);
|
---|
59 |
|
---|
60 | [m,n] = size(X);
|
---|
61 |
|
---|
62 | Y = R*X + repmat(T,[1 n]);
|
---|
63 |
|
---|
64 | if nargout > 1,
|
---|
65 |
|
---|
66 |
|
---|
67 | dYdR = zeros(3*n,9);
|
---|
68 | dYdT = zeros(3*n,3);
|
---|
69 |
|
---|
70 | dYdR(1:3:end,1:3:end) = X';
|
---|
71 | dYdR(2:3:end,2:3:end) = X';
|
---|
72 | dYdR(3:3:end,3:3:end) = X';
|
---|
73 |
|
---|
74 | dYdT(1:3:end,1) = ones(n,1);
|
---|
75 | dYdT(2:3:end,2) = ones(n,1);
|
---|
76 | dYdT(3:3:end,3) = ones(n,1);
|
---|
77 |
|
---|
78 | dYdom = dYdR * dRdom;
|
---|
79 |
|
---|
80 | end;
|
---|
81 |
|
---|
82 |
|
---|
83 |
|
---|
84 |
|
---|