|
0.簡(jiǎn)介 1.軸角(AA)轉(zhuǎn)化為其他形式 2.旋轉(zhuǎn)矩陣（R）轉(zhuǎn)化為其他形式 3.四元數(shù)（Q）轉(zhuǎn)化為其他形式 4.變換矩陣（T） 5.旋轉(zhuǎn)或變換應(yīng)用 6.實(shí)例 0、簡(jiǎn)介：
主要旋轉(zhuǎn)表達(dá)方法有：軸角(旋轉(zhuǎn)向量),、旋轉(zhuǎn)矩陣,、四元數(shù)、歐拉角,、變換矩陣[含旋轉(zhuǎn)和平移],；本講主要介紹相互之間如何轉(zhuǎn)化，以及如何進(jìn)行使用；下一講將介紹指數(shù)和對(duì)數(shù)映射,；
1,、軸角(旋轉(zhuǎn)向量AA)轉(zhuǎn)化為其他形式
根據(jù)羅德里格斯公式，n為旋轉(zhuǎn)軸,，單位長度,，θ是旋轉(zhuǎn)角度,，弧度制,；
function Rotation_Matrix=AAxisdToM(axis,angle)%axis:column vector,3-dimesion;旋轉(zhuǎn)軸，3維列向量,，單位長度%angle:scalar,rad;旋轉(zhuǎn)角度,，弧度制
%6-15行，確定輸入格式[rows,cols]=size(axis);[rows_a,cols_a]=size(angle);
if rows~=3 || cols~=1 error("rows numbers or column numbers are wrong");end
if rows_a~=1 || cols_a~=1 error("Angle must be a scalar(rad)");end
I=eye(3);%單位向量%羅德里格斯公式Rotation_Matrix=cos(angle)*I+(1-cos(angle))*axis*axis'+sin(angle)*getSkewSym(axis); end
根據(jù),，n為旋轉(zhuǎn)軸,，單位長度，θ是旋轉(zhuǎn)角度,，弧度制,；function Q=AAxisdToQ(axis,angle)%axis:column vector,3-dimesion;旋轉(zhuǎn)軸，三維列向量,，danwe度%angle:scalar,rad;旋轉(zhuǎn)角度,，弧度制%'在Maltab里表示轉(zhuǎn)置；
Q=[cos(angle/2),sin(angle/2)*axis'];
end
2,、旋轉(zhuǎn)矩陣（R）轉(zhuǎn)化為其他形式
旋轉(zhuǎn)矩陣->旋轉(zhuǎn)向量（軸角）：根據(jù)羅德里格斯公式,，兩邊取矩陣的跡trace,
旋轉(zhuǎn)軸上向量不會(huì)隨旋轉(zhuǎn)發(fā)生改變,，解矩陣方程Rn=1xn,，即旋轉(zhuǎn)軸向量是特征值為1對(duì)應(yīng)的；function [axis,angle]=RMatrixToAA(r)%axis:rotation_axis;輸出分為旋轉(zhuǎn)軸和旋轉(zhuǎn)角度%angle:rotation_angle;angle=acos((trace(r)-1)/2);[x,y]=eig(r);%求得特征值和特征向量y=diag(y);axis=x(:,y==1) %特征值為1對(duì)應(yīng)的特征向量end
根據(jù)公式,，旋轉(zhuǎn)矩陣為R, mij是R第i行第j列元素,；
function q=RMatrixToQ(R)%R:rotation_matri,3x3[rows,cols]=size(R);
if rows~=3 || cols~=3 error("Please Check the Rotation_Matrix");end
q0=sqrt(trace(R)+1)/2; q1=(R(2,3)-R(3,2))/4/q0; q2=(R(3,1)-R(1,3))/4/q0; q3=(R(1,2)-R(2,1))/4/q0; q=[q0,q1,q2,q3];end
3、四元數(shù)（Q）轉(zhuǎn)化為其他形式function r=QToM(q)[axis,angle]=QToAAxisd(q);r=AAxisdToM(axis,angle);end
function [axis,angle]=QToAAxisd(q)%q:unit quaterniond,1x4%axis:unit vector vector;%angle:rotation angleangle=2*acos(q(1));axis=q(2:4)'/sin(angle/2);end
變換矩陣與旋轉(zhuǎn)矩陣,、平移向量關(guān)系：
function TransMatrix=TransformMat(R,t)%R:rotation_matrix,3x3%t:translate vector,column vectorTransMatrix(1:3,1:3)=R;TransMatrix(1:3,4)=t;TransMatrix(4,1:3)=zeros(1,3);TransMatrix(4,4)=1;end
function v_rotated=Rotation(r,v,type)%v:初始向量,3xn,every column vector is a point;%r:q:quaterniond rotate,row vector,[z,x,y,z];rotation_matrix,3x3 matrix;%v_rotated:after rotate vector,every column is a point;%type:rotation type;%type---1:rotation_matrix,2:rotation_vector,3:quaterniond
%9-26行輸入檢查[rows,cols]=size(r);[rows_a,cols_a]=size(v);if type==1 if rows~=3 error("The first parameter must be 3x3 matrix(Rotaion_Matrix)"); endelseif type ==2 if rows~=1 || cols ~=4 error("The first parameter must be 1x4 matrix(Quaterniond)"); endelse error("The third parameter choose the type of rotation;1-Rotaion_Matrix,2-Quaterniond;") end
if rows_a~=3 error("rows numbers or column numbers are wrong[3x1]"); end
%根據(jù)旋轉(zhuǎn)形式,，進(jìn)行旋轉(zhuǎn),；if type==2 for i=1:cols_a v_rotated1=QProduct(QProduct(r,[0 v(:,i)']),QInverse(r)); v_rotated2=v_rotated1(2:4)'; vq(:,i)=v_rotated2; end v_rotated=vq;elseif type == 1 v_rotated=r*v;end
function v_transformed=Transform(T,v)%v:ori vector,3xn,every column vector is a point;%T:transform matrix,4x4 matrix;[rows,cols]=size(v);if rows~=3 error("the second parameter must be 3xn arry;") end%for i=1:colsv_trans=(T*[v;ones(1,cols)]);v_transformed=v_trans(1:3,:);end
%相機(jī)軌跡,，前三位是平移向量,，后四位是四元數(shù)，最后一位是四元數(shù)實(shí)部；tx ty tz qx qy qz qw0 0 0 0 0 0 1-0.288952827 0.222811699 -0.25202921 0.054562528 -0.312418818 -0.288284063 0.90349859-0.650643229 0.38382405 -0.501303971 -0.016285975 -0.159155473 -0.111743204 0.980774045
clcclear
data=importdata("cameraTrajectory.txt");%導(dǎo)入上面相機(jī)軌跡v=[1,5,0;3,8,10];%初始坐標(biāo)點(diǎn)
plot3(v(:,1),v(:,2),v(:,3),'r');axis([-30 30 -30 30 -30 30]);grid on;hold on;for i=2:3 t=data.data(i,1:3); q=[data.data(i,7),data.data(i,4:6)]; r=QToM(q); TransMatrix=TransformMat(r,t); v_updated=Transform(TransMatrix,v') v=v_updated'; plot3(v(:,1),v(:,2),v(:,3),'r');end
|
