网站建设与管理教学计划/互联网优化

0、简介：

主要介绍常用李代数和李群的转化方法，具体公式参考《slam入门-李群和李代数-第2讲》
李代数：so(3),se(3)
李 群：SO(3),SE(3)

1.1指数映射：so(3)->SO(3)

function RMatrix=Exp(so)
%so:Li_algebra,3-dimension column vector;
%RMatrix:li_group,3x3 matrix;
%Exp mapping
angle=norm(so);
axis=so/angle;
RMatrix=AAToRMatrix(axis,angle);
end

1.2指数映射：se(3)->SE(3)

function TMatrix=Exp_se(se)
%se:se(3),Li_algebra,6-dimension column vector,first 3 are rotate,last 3 are pretranslate;
%TMatrix:li_group,4x4 matrix;
%Exp mapping
RMatrix=Exp(se(1:3));
angle=norm(se(1:3));
axis=se(1:3)/angle;
J=sin(angle)/angle*eye(3)+(1-sin(angle)/angle)*axis*axis'+(1-cos(angle))/angle*getSkewSym(axis);
t=J*se(4:6);
TMatrix(1:3,1:3)=RMatrix;
TMatrix(1:3,4)=t;
TMatrix(4,1:4)=[0 0 0 1];
end

1.3对数映射:SO(3)->so(3)

function so=Log(RMatrix)
%so:Li_algebra,3-dimension column vector;
%RMatrix:li_group,3x3 matrix;
%Log mapping
[axis,angle]=RMatrixToAA(RMatrix);;
so=angle*axis;
end

1.4对数映射:SE(3)->se(3)

function se=Log_se(TMatrix)
%se:se(3),Li_algebra,6-dimension column vector,first 3 are rotate,last 3 are pretranslate;
%TMatrix:li_group,4x4 matrix;
%Log mapping
RMatrix=TMatrix(1:3,1:3);
[axis,angle]=RMatrixToAA(RMatrix);
so=angle*axis;
J=sin(angle)/angle*eye(3)+(1-sin(angle)/angle)*axis*axis'+(1-cos(angle))/angle*getSkewSym(axis);
rou=inv(J)*TMatrix(1:3,4);
se=[so;rou];
end

1.5辅助函数:向量->反对称矩阵

function Matrix=getSkewSym(v)
%v:column vector,is a point
%Matrix:is the skew symmetric matrix of vector
[rows,cols]=size(v);
if rows~=3error("It must be a column vector"); 
end
Matrix=[0, -v(3), v(2);v(3), 0, -v(1);-v(2), v(1), 0];
end

1.6辅助函数:反对称矩阵->向量

function [v1,v2,v3]=getUnSkewSym(matrix)
%v:column vector,is a point
%Matrix:is the skew symmetric matrix of vector
[rows,cols]=size(matrix);
if rows~=3 || cols~=3error("It must be 3x3 matrix"); 
end
if matrix(1,1)~=0 || matrix(2,2)~=0 || matrix(3,3)~=0error("Diag must be 0"); 
end
if matrix(1,2)~=-1*matrix(2,1) || matrix(1,3)~=-1*matrix(3,1) ||  matrix(2,3)~=-1*matrix(3,2) error("It must be SkewSymMatrix"); 
end
v1=-1*matrix(2,3);
v2=matrix(1,3);
v3=-1*matrix(1,2);

1.7 轴角->旋转矩阵

function RMatrix=AAToRMatrix(axis,angle)
RMatrix=cos(angle)*eye(3)+(1-cos(angle))*axis*axis.'+sin(angle)*getSkewSym(axis);
end

1.8 旋转矩阵->轴角

function [axis,angle] = RMatrixToAA(Rmatrix)
angle=acos((trace(Rmatrix)-1)/2);
[v,~]=eig(Rmatrix);
axis=v(:,1);
end