旋转矩阵

三维旋转矩阵

三维旋转特性

给定单位向量u和旋转角度φ，则R(φ,u)表示绕单位向量u旋转φ角度。

R(0,u)表示旋转零度。

R(φ,u)= R(−φ,−u)。

R(π+φ,u)= R(π−φ,−u)。

如果φ=0，则u为任意值。

如果0<φ<π,则u唯一确定。

如果φ= π，则符号不是很重要。因为- π和π是一致的，结果相同，动作不同。

由旋转矩阵求旋转角和旋转轴

每一个三维旋转都能有旋转轴和旋转角唯一确定，好多方法都可以从旋转矩阵求出旋转轴和旋转角，下面简单介绍用特征值和特征向量确定旋转轴和旋转角的方法。

将旋转矩阵作用在旋转轴上，则旋转轴还是原来的旋转轴，公式表示如下：

Ru=u

转化得：

Ru=Iu =>(R−I)u=0

可以确定的是u在R-I的零空间中，角度可有下面的公式求得，Tr表示矩阵的迹：

Tr(R)=1+2cosθ

从旋转轴和旋转角求旋转矩阵

假设给定单位向量u=(u

x，u

y，

u z)

T

，并且u为单位向量即：

u x2+u y2+u z2=1，给定绕u旋转的角度θ，可以得出旋转矩阵R：

R=[cosθ+u x2(1−cosθ)u x u y(1−cosθ)−u z sinθu x u z(1−cosθ)+u y sinθ

u y u x(1−cosθ)+u z sinθcosθ+u y2(1−cosθ)u y u z(1−cosθ)−u x sinθ

u z u x(1−cosθ)−u y sinθu z u y(1−cosθ)+u x sinθcosθ+u z2(1−cosθ)

]

上面的公式等价于：

R=cosθI+sinθ[u]×+(1−cosθ)u⊗u

其中[u]×是单位向量u的叉乘矩阵，⊗表示张量积，I是单位向量. 这是罗德里格斯旋转方程的矩阵表示。下面给出叉乘和张量积的公式：

u⊗u=[u x2u x u y u x u z

u x u y u y2u y u z u x u z u y u z u z2

]

[u]×=[

0−u z u y u z0−u x −u y u x0

]

面向旋转轴，逆时针旋转为正方向，此时旋转矩阵的行列式为1，反之为反方向，旋转矩阵的行列式为-1。

奇异值分解

奇异值分解可以返回两组标准正交向量基和一组拉伸向量，可以用于旋转矩阵的校正，如果在求解旋转矩阵的过程中引入了误差，使旋转动作中产生微量的拉伸，可以使用svd进行校正，将返回的拉伸向量用单位向量替换，并进行逆运算得到校正后的旋转矩阵。

三维坐标系转化—由点集求解

已知一系列点在A坐标系下的坐标和B坐标系下对应的坐标，求两个坐标系之间的转换关系。

把RT作为未知数表示出来

3 3个方程12个未知数，至少需要取4个对应点，联立方程得AX=b，所以，X=A\b。matlab代码如下：

1.function [ X ] = ReadX( path )

2.%READXWORLD 此处显示有关此函数的摘要

3.% 此处显示详细说明

4.%读入txt中的格式化数据:(1,2,3)

5.%type为1组成n列，type为其他组成1列

6.[x y z] = textread(path,'(%f,%f,%f)');

7.X=[x y z]';

8.end

1.function [ R T ] = SolveRT( path1,path2 )

2.%SOLVERT 此处显示有关此函数的摘要

3.% 此处显示详细说明

4.

5.XWorld=ReadX(path1);

6.XCamera=ReadX(path2);

7.

8.[R T]=solveRT(XWorld,XCamera);

9.

10.end

11.function [R T] = solveRT(XWorld,XCamera)

12.%n是列数即有多少组点

13.n=size(XWorld,2);

14.zero=zeros(1,3);

15.one=eye(3,3);

16.x=[];

17.X=[];

18.Y=[];

19.for i=1:n

20. x=XWorld(:,i)';

21. xx=[x zero zero;zero x zero;zero zero x];

22. xx=[xx one];

23. X=[X;xx];

24. Y=[Y;XCamera(:,i)];

25.end

26.P=X\Y;

27.R=P(1:9);

28.R=reshape(R,3,3);

29.R=R';

30.T=P(10:12);

31.end

三维坐标系转化—由坐标系求解

对两个空间三维直角坐标系O1-X1Y1Z1和O2-X2Y2Z2，要实现空间直角坐标系O1-

X1Y1Z1到O2-X2Y2Z2的转换，首先通过坐标原点平移使O1和O2重合，然后通过旋转实现O1-X1Y1Z1到O2-X2Y2Z2的转换。

两个坐标系相乘即是旋转矩阵。已验证，待证明：