compose_motion.m

function [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2);

% Rotations:

[R1,dR1dom1] = rodrigues(om1);
[R2,dR2dom2] = rodrigues(om2);

R3 = R2 * R1;

[dR3dR2,dR3dR1] = dAB(R2,R1);

[om3,dom3dR3] = rodrigues(R3);

dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1;
dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2;

dom3dT1 = zeros(3,3);
dom3dT2 = zeros(3,3);


% Translations:

T3t = R2 * T1;

[dT3tdR2,dT3tdT1] = dAB(R2,T1);

dT3tdom2 = dT3tdR2 * dR2dom2;


T3 = T3t + T2;

dT3dT1 = dT3tdT1;
dT3dT2 = eye(3);

dT3dom2 = dT3tdom2;
dT3dom1 = zeros(3,3);


return;

% Test of the Jacobians:

om1 = randn(3,1);
om2 = randn(3,1);
T1 = 10*randn(3,1);
T2 = 10*randn(3,1);

[om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2);


dom1 = randn(3,1) / 1000;
dom2 = randn(3,1) / 1000;
dT1  = randn(3,1) / 10000;
dT2  = randn(3,1) / 10000;

[om3r,T3r] = compose_motion(om1+dom1,T1+dT1,om2+dom2,T2+dT2);

om3p = om3 + dom3dom1*dom1 +  dom3dT1*dT1 + dom3dom2*dom2 +  dom3dT2*dT2;
T3p  =  T3 + dT3dom1*dom1  +  dT3dT1*dT1  + dT3dom2*dom2  +  dT3dT2*dT2;

norm(om3r - om3) / norm(om3r - om3p)
norm(T3r - T3) / norm(T3r - T3p)