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

Last change on this file since 849 was 810, checked in by g7moreau, 10 years ago
  • Add license
File size: 2.6 KB
Line 
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
19function [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
46if 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;
55end;
56
57
58[R,dRdom] = rodrigues(om);
59
60[m,n] = size(X);
61
62Y = R*X + repmat(T,[1 n]);
63
64if nargout > 1,
65   
66
67dYdR = zeros(3*n,9);
68dYdT = zeros(3*n,3);
69
70dYdR(1:3:end,1:3:end) =  X';
71dYdR(2:3:end,2:3:end) =  X';
72dYdR(3:3:end,3:3:end) =  X';
73
74dYdT(1:3:end,1) =  ones(n,1);
75dYdT(2:3:end,2) =  ones(n,1);
76dYdT(3:3:end,3) =  ones(n,1);
77
78dYdom = dYdR * dRdom;
79
80end;
81
82
83
84
Note: See TracBrowser for help on using the repository browser.