|
0,、簡介：
主要介紹常用李代數(shù)和李群的轉(zhuǎn)化方法,，具體公式參考《slam入門-李群和李代數(shù)-第2講》李代數(shù)：so(3),se(3)
李 群：SO(3),SE(3) 1.1指數(shù)映射： 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指數(shù)映射： 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對數(shù)映射: 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對數(shù)映射: 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輔助函數(shù): 向量->反對稱矩陣 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輔助函數(shù): 反對稱矩陣->向量 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);
|
