【原】slam代码实践[Matlab]-李群和李代数

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 mappingangle=norm(so);axis=so/angle;RMatrix=AAxisdToM(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 mappingRMatrix=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 mappingRMatrix=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~=3   error("It must be a column vector"); endMatrix=[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~=3   error("It must be 3x3 matrix"); endif matrix(1,1)~=0 || matrix(2,2)~=0 || matrix(3,3)~=0   error("Diag must be 0"); endif 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"); endv1=-1*matrix(2,3);v2=matrix(1,3);v3=-1*matrix(1,2);