Codes: MODERN ROBOTICS_Ch.3_矩阵指数映射与对数映射的代码实现

%% Matrix exponential for so3
% 根据wtheta_mat，求取旋转矩阵
function [R]=MatExp3r(wtheta_mat)
    % 求出w与theta
    wtheta = so3ToVecr(wtheta_mat);
    theta = norm(wtheta);
    w = wtheta./theta;
    % 求w的矩阵表示w_mat
    w_mat = vecToso3r(w);
    % 求矩阵指数映射
    R= eye(3)+sin(theta).* w_mat+(1-cos(theta)).* w_mat^2;
end

%% 矩阵对数算法，根据旋转矩阵R，求旋转指数坐标的矩阵表示wtheta_mat
% R to so3mat, namely, so3omghat.*theta
function wtheta_mat = MatLog3r(R)
    if isequal(R,eye(3))
        theta = 0;
        fprintf(‘omghat is undefined‘);
        wtheta_mat = zeros(3);
    elseif trace(R)==-1
        theta = pi;
        if ~NearZero(1+R(3,3))
            w =(1/sqrt(2*(1+R(3,3))))...
                    .*[R(1,3);R(2,3);1+R(3,3)]
        elseif ~NearZero(1+R(2,2))
            w =(1/sqrt(2*(1+R(2,2))))...
                    .*[R(1,2);1+R(2,2);R(3,2)]
        elseif ~NearZero(1+R(3,3))
            w =(1/sqrt(2*(1+R(1,1))))...
                    .*[1+R(1,1);R(2,1);R(3,1)]
        end
        w_mat = vecToso3r(w);
        wtheta_mat = w_mat.*theta;
    else
        theta = acos((trace(R)-1)/2);
        w_mat = (1/(2*sin(theta))).*(R-R‘);
        wtheta_mat = w_mat.*theta;
    end
end

%% 根据指数坐标的矩阵表示stheta_mat，求变换矩阵T
function T = MatExp6r(stheta_mat)
    % 从指数坐标的矩阵表示se3中，解出螺旋轴s、theta
    stheta = se3ToVecr(stheta_mat);
    wtheta = stheta(1:3);
    vtheta = stheta(4:6);
    % 根据wtheta是否为零，判断theta的取值
    if ~NearZero(norm(wtheta))
        theta = norm(wtheta);
    else
        theta = norm(vtheta);
    end
    % 从stheta中求单位螺旋轴s
    s = stheta./theta;
    w = s(1:3);
    v = s(4:6);
    % 求旋转轴向量w的矩阵表示w_mat
    w_mat = vecToso3r(w);
    % 求旋转指数坐标wtheta的矩阵表示wtheta_mat
    wtheta_mat = vecToso3r(wtheta);
    % 求指数坐标的矩阵表示对应的指数映射，R
    R = MatExp3r(wtheta_mat);
    Gv = (eye(3).*theta+(1-cos(theta)).*w_mat+(theta-sin(theta)).*w_mat^2)*v;
    T=[R, Gv; 
       0,0,0,1];
end

%% 根据变换矩阵T,求螺旋指数坐标stheta_mat
function stheta_mat = MatLog6r(T)
    [R,p] = TransToRpr(T);
    if isequal(eye(3), R)
        w = zeros(3,1);
        theta = norm(p);
        v = p./theta;
    else
        wtheta_mat = MatLog3r(R);
        wtheta = so3ToVecr(wtheta_mat);
        theta = norm(wtheta);
        w = (wtheta)‘./theta;
        w_mat = vecToso3r(w);
        v = ((1/theta).*eye(3)-1/2.*w_mat+(1/theta-1/2*cot(theta/2)).*w_mat^2)*p;
    end
    s = [w; v];
    stheta = s.*theta;
    stheta_mat = VecTose3r(stheta);
end