function w = dexpinvSE3 (sigma, v) % % Calculate dexp_sigma^{-1}(v) %INPUT % sigma - (1:3) is the so(3) part of signa, (4:6) the translation part % v - (1:3) is the so(3) part of v, (4:6) - the translation part of v % OUTPUT % w (1:3) is the so(3) part of w, (4:6) - the translation part of w A = sigma(1:3); a = sigma(4:6); hat=@(xi) [0, -xi(3),xi(2); xi(3), 0, -xi(1); -xi(2), xi(1), 0]; rho = A'*a; g1 = @(z) -0.5; g1tilde = @(z) 0; g2 = @(z) (1-z/2*cot(z/2))/(z^2); g2low = @(z) 1/12 + z^2/720 + z^4/(30240)+z^6/(1209600); g2prime = @(z) 1/z^2 * (-0.5*cot(z/2)-z/2*(-0.5-0.5*cot(z/2)^2))-... 2/z^3 * (1-z/2 * cot(z/2)); g2tilde = @(z) rho/z * g2prime(z); g2tildelow = @(z) rho*(1/360+z^2/7560); f0 = 1; alpha = norm(A,2); mat = zeros(3); tol = 1e-16; if (alpha)>tol Mat = f0*eye(6) + [g1(alpha)*hat(A), mat; g1(alpha)*hat(a)+... g1tilde(alpha)*hat(A), g1(alpha)*hat(A)] + ... [hat(A), mat;hat(a), hat(A)] * [g2(alpha)*hat(A), mat; ... g2(alpha)*hat(a)+g2tilde(alpha)*hat(A), g2(alpha)*hat(A)]; w = Mat * v; else Mat = f0*eye(6) + [g1(alpha)*hat(A), mat; g1(alpha)*hat(a)+... g1tilde(alpha)*hat(A), g1(alpha)*hat(A)] + ... [hat(A), mat;hat(a), hat(A)] * [g2low(alpha)*hat(A), mat; ... g2low(alpha)*hat(a)+g2tildelow(alpha)*hat(A), g2low(alpha)*hat(A)]; w = Mat * v; end end