Homogeneous transformation

%% Linear Algebra things

%

% 1. Make T from three points 'ps2R'

% 2, Local transformation (translation + rotation): post-multiplication

%

ccc

%% 1. Make T from three points 'ps2R'

ccc

p1 = -1+2*rand(1,3);

p2 = -1+2*rand(1,3);

p3 = -1+2*rand(1,3);

% Get T from p1, p2, and p3

v1 = p2-p1; % v1

v2 = p3-p1; % v2

T = pr2t([0,0,0],ps2R(p1,p2,p3));

% plot two vectors and a T matrix

figure(1); hold on;

plot_T(T,'');

plot_sphere(v1,struct('subfig_idx',1,'r',0.1,'face_color','r'));

plot_sphere(v2,struct('subfig_idx',2,'r',0.1,'face_color','g'));

axis equal; rotate3d on; grid on;

xlabel('X','fontSize',20); ylabel('Y','fontSize',20); zlabel('Z','fontSize',20);

view(82,16);

%% 2, Local transformation (translation + rotation): post-multiplication

ccc

rng(0);

% Initial T

p_rand = -1+2*rand(1,3);

R_rand = rpy2r(360*rand(1,3));

T_init = pr2t(p_rand,R_rand);

% Do local transformation

for tick = 1:1000

p_trsf_x = [tick*0.001,0,0]; rpy_trsf_x = [tick,0,0];

T_trsf_x = T_init*pr2t(p_trsf_x,rpy2r(rpy_trsf_x));

p_trsf_y = [0,tick*0.001,0]; rpy_trsf_y = [0,tick,0];

T_trsf_y = T_init*pr2t(p_trsf_y,rpy2r(rpy_trsf_y));

p_trsf_z = [0,0,tick*0.001]; rpy_trsf_z = [0,0,tick];

T_trsf_z = T_init*pr2t(p_trsf_z,rpy2r(rpy_trsf_z));

% plot

plot_T(pr2t([0,0,0],eye(3,3)),struct('subfig_idx',1,'fig_size',[0.1,0.3,0.4,0.6],...

'len',0.5,'lw',3,'SHOW_TEXT',0,'text_str','Global','text_fs',20));

plot_T(T_init,struct('subfig_idx',2,'len',0.3,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{org}','text_fs',20));

plot_T(T_trsf_x,struct('subfig_idx',3,'len',0.2,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{trsf x}','text_fs',10));

plot_T(T_trsf_y,struct('subfig_idx',4,'len',0.2,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{trsf y}','text_fs',10));

plot_T(T_trsf_z,struct('subfig_idx',5,'len',0.2,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{trsf z}','text_fs',10));

title(sprintf('[%d] PRE-multiplications make LOCAL transforms.',tick),...

'fontSize',30)

if tick == 1

axis equal; axis([-1.5,1.5,-1.5,1.5,-1.5,1.5]); grid on; view(82,16); rotate3d on;

xlabel('X','fontSize',20); ylabel('Y','fontSize',20); zlabel('Z','fontSize',20);

end

drawnow;

end

fprintf(2,'Done.\n');

%% 2, Global transformation (translation + rotation): pre-multiplication

ccc

rng(0);

% Initial T

p_rand = -1+2*rand(1,3);

R_rand = rpy2r(360*rand(1,3));

T_init = pr2t(p_rand,R_rand);

% Do local transformation

for tick = 1:1000

p_rot_x = [0,0,0]; rpy_rot_x = [2*tick,0,0];

T_rot_x = pr2t(p_rot_x,rpy2r(rpy_rot_x))*T_init;

p_trsl_y = [0,tick*0.001,0]; rpy_trsl_y = [0,0,0];

T_trsl_y = pr2t(p_trsl_y,rpy2r(rpy_trsl_y))*T_init;

p_trsl_z = [0,0,tick*0.001]; rpy_trsl_z = [0,0,0];

T_trsl_z = pr2t(p_trsl_z,rpy2r(rpy_trsl_z))*T_init;

% plot

plot_T(pr2t([0,0,0],eye(3,3)),struct('subfig_idx',1,'fig_size',[0.1,0.3,0.4,0.6],...

'len',0.5,'lw',3,'SHOW_TEXT',0,'text_str','Global','text_fs',20));

plot_T(T_init,struct('subfig_idx',2,'len',0.3,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{org}','text_fs',20));

plot_T(T_rot_x,struct('subfig_idx',3,'len',0.2,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{rot x}','text_fs',10));

plot_T(T_trsl_y,struct('subfig_idx',4,'len',0.2,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{trsl y}','text_fs',10));

plot_T(T_trsl_z,struct('subfig_idx',5,'len',0.2,'lw',2,...

'SHOW_TEXT',1,'text_str','T_{trsl z}','text_fs',10));

title(sprintf('[%d] POST-multiplications make GLOBAL transforms.',tick),...

'fontSize',30)

if tick == 1

axis equal; axis([-1.5,1.5,-1.5,1.5,-1.5,1.5]); grid on; view(82,16); rotate3d on;

xlabel('X','fontSize',20); ylabel('Y','fontSize',20); zlabel('Z','fontSize',20);

end

drawnow;

end

fprintf(2,'Done.\n');

%%

function fig = plot_T(T,opt)

%

% Plot a 4x4 transformation matrix

%

persistent h

% Make enough handlers at the first

if isempty(h), for i = 1:100, for j = 1:1000, h{i,j}.first_flag = true; end; end; end

% Parse input arguments

fig_idx = getfield_safe(opt,'fig_idx',1,'plot_T');

subfig_idx = getfield_safe(opt,'subfig_idx',1,'plot_T');

fig_size = getfield_safe(opt,'fig_size','','plot_T');

len = getfield_safe(opt,'len',1.0,'plot_T');

lw = getfield_safe(opt,'lw',2,'plot_T');

line_style = getfield_safe(opt,'line_style','-','plot_T');

PLOT_SPHERE = getfield_safe(opt,'PLOT_SPHERE',0,'plot_T');

if PLOT_SPHERE

r = getfield_safe(opt,'r',0.2,'plot_T');

face_color = getfield_safe(opt,'face_color','g','plot_T');

face_alpha = getfield_safe(opt,'face_alpha',0.3,'plot_T');

end

PLOT_COORD = getfield_safe(opt,'PLOT_COORD',1,'plot_T');

SHOW_TEXT = getfield_safe(opt,'SHOW_TEXT',1,'plot_T');

if SHOW_TEXT

text_str = getfield_safe(opt,'text_str','','plot_T');

text_fs = getfield_safe(opt,'text_fs',15,'plot_T');

end

% Get p and R

p = T([1,2,3],4);

R = T([1,2,3],[1,2,3]);

ex = R(:,1); ey = R(:,2); ez = R(:,3);

if h{fig_idx,subfig_idx}.first_flag || ... % first flag

~ishandle(h{fig_idx,subfig_idx}.fig)

h{fig_idx,subfig_idx}.first_flag = false;

% Initialize figure

h{fig_idx,subfig_idx}.fig = figure(fig_idx);

if ~isempty(fig_size)

set_fig_size(h{fig_idx,subfig_idx}.fig,fig_size);

end

hold on;

% Plot coordinate

if PLOT_COORD

h{fig_idx,subfig_idx}.cx = plot3([p(1),p(1)+len*ex(1)],[p(2),p(2)+len*ex(2)],[p(3),p(3)+len*ex(3)],...

'r','LineWidth',lw,'LineStyle',line_style);

h{fig_idx,subfig_idx}.cy = plot3([p(1),p(1)+len*ey(1)],[p(2),p(2)+len*ey(2)],[p(3),p(3)+len*ey(3)],...

'g','LineWidth',lw,'LineStyle',line_style);

h{fig_idx,subfig_idx}.cz = plot3([p(1),p(1)+len*ez(1)],[p(2),p(2)+len*ez(2)],[p(3),p(3)+len*ez(3)],...

'b','LineWidth',lw,'LineStyle',line_style);

end

% Plot sphere

if PLOT_SPHERE

[x,y,z] = ellipsoid(p(1),p(2),p(3), r, r, r, 15);

h{fig_idx,subfig_idx}.surf = ...

surf(x,y,z,...

'EdgeColor','none','FaceColor',face_color,'FaceAlpha',face_alpha);

end

% Show text

if SHOW_TEXT

h{fig_idx,subfig_idx}.text = text(p(1),p(2),p(3),text_str,'fontsize',text_fs,...

'VerticalAlignment','middle','HorizontalAlignment','center');

end

else

% Update coordinate

if PLOT_COORD

h{fig_idx,subfig_idx}.cx.XData = [p(1),p(1)+len*ex(1)];

h{fig_idx,subfig_idx}.cx.YData = [p(2),p(2)+len*ex(2)];

h{fig_idx,subfig_idx}.cx.ZData = [p(3),p(3)+len*ex(3)];

h{fig_idx,subfig_idx}.cy.XData = [p(1),p(1)+len*ey(1)];

h{fig_idx,subfig_idx}.cy.YData = [p(2),p(2)+len*ey(2)];

h{fig_idx,subfig_idx}.cy.ZData = [p(3),p(3)+len*ey(3)];

h{fig_idx,subfig_idx}.cz.XData = [p(1),p(1)+len*ez(1)];

h{fig_idx,subfig_idx}.cz.YData = [p(2),p(2)+len*ez(2)];

h{fig_idx,subfig_idx}.cz.ZData = [p(3),p(3)+len*ez(3)];

end

% Update sphere

if PLOT_SPHERE

[x,y,z] = ellipsoid(p(1),p(2),p(3), r, r, r, 15);

h{fig_idx,subfig_idx}.surf.XData = x;

h{fig_idx,subfig_idx}.surf.YData = y;

h{fig_idx,subfig_idx}.surf.ZData = z;

end

% Update text

if SHOW_TEXT

h{fig_idx,subfig_idx}.text.Position = p;

h{fig_idx,subfig_idx}.text.String = text_str;

end

fig = h{fig_idx,subfig_idx}.fig;

function plot_sphere(pos,opt)

%

% Plot 3d sphere with handler

%

persistent h

% Make enough handlers at the first

if isempty(h)

for i = 1:10

for j = 1:1000

h{i,j}.first_flag = true;

end

% Parse arguments

fig_idx = getfield_safe(opt,'fig_idx',1,'plot_sphere');

subfig_idx = getfield_safe(opt,'subfig_idx',1,'plot_sphere');

r = getfield_safe(opt,'r',1,'plot_sphere');

face_color = getfield_safe(opt,'face_color','b','plot_sphere');

face_alpha = getfield_safe(opt,'face_alpha',0.3,'plot_sphere');

[x, y, z] = ellipsoid(pos(1), pos(2), pos(3), r, r, r, 15);

if h{fig_idx,subfig_idx}.first_flag || ...% first flag

~ishandle(h{fig_idx,subfig_idx}.fig)

h{fig_idx,subfig_idx}.first_flag = false;

h{fig_idx,subfig_idx}.fig = figure(fig_idx);

h{fig_idx,subfig_idx}.surf = ...

surf(x,y,z, 'EdgeColor','none','FaceColor',face_color,...

'FaceAlpha',face_alpha);

else

h{fig_idx,subfig_idx}.surf.XData = x;

h{fig_idx,subfig_idx}.surf.YData = y;

h{fig_idx,subfig_idx}.surf.ZData = z;

end

function val = getfield_safe(opt,field,default_val,debug_str)

%

% Get the field item if exists, and return default otherwise.

%

global VERBOSE

if ~exist('VERBOSE','var')

VERBOSE = 0;

end

if isfield(opt,field) % if field exists, use it

val = getfield(opt,field);

else % otherwise, use the default value

if VERBOSE

fprintf(2,'[%s][%s] field does not exist. Using [%s] instead. \n',...

debug_str,field,num2str(default_val));

end

val = default_val;

end

function T = pr2t(p,R)

%

% Get T from p and R

%

T = [R, reshape(p,[3,1]);0,0,0,1];

function R = rpy2r(rpy_deg)

%

% roll pitch yaw (degree) to rotation matrix

%

roll_deg = rpy_deg(1);

pitch_deg = rpy_deg(2);

yaw_deg = rpy_deg(3);

D2R = pi/180;

phi = roll_deg*D2R;

theta = pitch_deg*D2R;

psi = yaw_deg*D2R;

cPhi = cos(phi); sPhi = sin(phi);

cTheta = cos(theta); sTheta = sin(theta);

cPsi = cos(psi); sPsi = sin(psi);

R = [cPsi*cTheta, -sPsi*cPhi+cPsi*sTheta*sPhi, sPsi*sPhi+cPsi*sTheta*cPhi ; ...

sPsi*cTheta, cPsi*cPhi+sPsi*sTheta*sPhi, -cPsi*sPhi+sPsi*sTheta*cPhi ; ...

-sTheta, cTheta*sPhi, cTheta*cPhi];

function R = ps2R(p1,p2,p3)

%

% Three positions to R

%

v1 = p2-p1; % v1

v2 = p3-p1; % v2

u1 = v1 / norm(v1);

u2 = v2 - proj_vec(u1,v2);

u3 = cross(u1,u2);

e1 = u1 / norm(u1);

e2 = u2 / norm(u2);

e3 = u3 / norm(u3);

R = [e1',e2',e3'];