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