Apply rigid body transformation to 'X' Xout = vb_rigid_transform(X, para) --- Input X : 3D-coordinate to be transformed (Nx3 vector) para : [ dX ; th ] rigid body transformation parameter (6x1 vector) dX : Translation parameter of 3 directions (3x1 vector) th : Rotation angle around x, y, z axis [radian] (3x1 vector) --- Output Xout : transformed coordinate (Nx3 vector) Copyright (C) 2011, ATR All Rights Reserved. License : New BSD License(see VBMEG_LICENSE.txt)
0001 function Xout = vb_rigid_transform(X, para) 0002 % Apply rigid body transformation to 'X' 0003 % Xout = vb_rigid_transform(X, para) 0004 % --- Input 0005 % X : 3D-coordinate to be transformed (Nx3 vector) 0006 % para : [ dX ; th ] rigid body transformation parameter (6x1 vector) 0007 % dX : Translation parameter of 3 directions (3x1 vector) 0008 % th : Rotation angle around x, y, z axis [radian] (3x1 vector) 0009 % --- Output 0010 % Xout : transformed coordinate (Nx3 vector) 0011 % 0012 % 0013 % Copyright (C) 2011, ATR All Rights Reserved. 0014 % License : New BSD License(see VBMEG_LICENSE.txt) 0015 0016 dX = para(1:3); 0017 th = para(4:6); 0018 0019 Rot_x = [1 0 0 ;... 0020 0 cos(th(1)) -sin(th(1));... 0021 0 sin(th(1)) cos(th(1));... 0022 ]; 0023 0024 Rot_y = [ cos(th(2)) 0 -sin(th(2));... 0025 0 1 0 ;... 0026 sin(th(2)) 0 cos(th(2));... 0027 ]; 0028 0029 Rot_z = [ cos(th(3)) -sin(th(3)) 0;... 0030 sin(th(3)) cos(th(3)) 0;... 0031 0 0 1 ;... 0032 ]; 0033 0034 % Rotation matrix 0035 Rot = Rot_z * Rot_y * Rot_x; 0036 0037 % Rotation 0038 Xout = X * Rot; 0039 0040 % Translation 0041 Xout(:,1) = Xout(:,1) + dX(1); 0042 Xout(:,2) = Xout(:,2) + dX(2); 0043 Xout(:,3) = Xout(:,3) + dX(3); 0044