1function Ad_g = calculateAdjoint(g)
 2%#codegen
 3
 4rotation = g(1:3,1:3);
 5pos = g(1:3,4);
 6
 7Ad_g = [rotation, skew(pos)*rotation; zeros(3,3), rotation];
 8
 9end