Lie Algebra to Lie Group Mapping.

Question

When I map a lie algebra vector in se3 to SE3 using exponentiation and map it back to se3 using log, why do I get significantly different results? I followed this and coded an implementation in matlab. Is there something else I should know?

Here is my code for se3 -> SE3

 function [ SE3 ] = se3_SE3( se3 )
    %se3_SE3 Exponential Mapping from Lie Algebra to Lie Group
    %   se3 is a 1x6 Column Vector of the form=[v1 v2 v3 w1 w2 w3] which is
    %   defined using 6 Generator Matrices(4x4)
    %   each of the six elements on multiplication with the generator matrices
    %   as follows give the complete matrix:
    %   se3 = v1*G1 + v2*G2 + v3*G3 + w1*G4 + w2*G5 + w3*G6
    %   To map se3 to SE3 we need to perform e^(se3)
    %   This can be done by following the algorithm:
    %   Algorithm
    %   
    %
    %
    %
        w=se3(4:6)';
        u=se3(1:3)';
        wx=[0 -w(3) w(2);w(3) 0 -w(1);-w(2) w(1) 0];
        theta=sqrt(w'*w);
        if(theta~=0),
            A=sin(theta)/theta;
            B=(1-cos(theta))/(theta^2);
            C=(1-A)/(theta^2);
        else
            A=0;
            B=0;
            C=0;  
        end
        R=eye(3)+(A*wx)+(B*(wx*wx));
        V=eye(3)+B*wx+C*(wx*wx);
        Vp=V*u;
        SE3=zeros(4);
        SE3(1:3,1:3)=R;
        SE3(1:3,4)=Vp;
        SE3(4,4)=1;
    end

Here is my code for SE3 -> se3

function [ se3] = SE3_se3_back( SE3 )
%UNTITLED Summary of this function goes here
%   Detailed explanation goes here
    R=SE3(1:3,1:3);
    theta=acos((trace(R)-1)/2);
    lnR=(theta/2*sin(theta))*(R-R');
    w=[-lnR(2,3) lnR(1,3) -lnR(1,2)];
    wx=[0 -w(3) w(2);w(3) 0 -w(1);-w(2) w(1) 0];
    if(theta==0),
        Vin=eye(3);
    else
        A=sin(theta)/theta;
        B=(1-cos(theta))/(theta^2);
        Vin=eye(3)-(1/2)*wx+(1/(theta^2))*(1-(A/(2*B)))*(wx*wx);
    end
    u=Vin*SE3(1:3,4);
    se3=[u' w];

end

example:

SE3_se3_back(se3_SE3([0.1 0.1 0.1 0.01 0.1 0.1]))
returns 0.0997    0.1044    0.0956    0.0002    0.0020    0.0020

Answer 1

The line

lnR=(theta/2*sin(theta))*(R-R')

should be changed to

lnR=(theta/(2*sin(theta)))*(R-R')