3D视觉（三）刚体运动及matlab实现

刚体运动（rigid motion）：在三维空间中， 几何体作旋转和平移运动，称为刚体运动。
刚体运动可以理解为保持长度、角度、面积等特征不变的仿射变换，即内积和度量不变；具有物理意义的量，如梯度，散度和旋度都保持不变。从坐标变换上看， 旋转矩阵为正交矩阵。从群的角度看，刚体变换全体构成一个群。

（一）补一下数学知识：

正交矩阵

（1）正交矩阵每一列都是单位矩阵，并且两两正交。最简单的正交矩阵就是单位阵。
（2）正交矩阵的逆（inverse）等于正交矩阵的转置（transpose）。同时可以推论出正交矩阵的行列式的值为正负1。

李代数and李群

(1)李代数（Lie algebra）是一类重要的非结合代数，最初是由19世纪挪威数学家索菲斯·李创立李群时引进的一个数学概念。假设L是域F上的向量空间。如果L上有一个运算L×L→L，(x,y)→[x,y]满足以下三个条件，则称L是一个李代数。
（1）这个运算是双线性的，即 [ax+by,cz+dw]=ac[x,z]+cb[y,z]+ad[x,w]+bd[y,w]。
（2）[x,x]=0，对L中任意元素x∈L。
（3）[x,[y,z]]+[y,[z,x]]+[z,[x,y]]=0，对所有L中元素x，y，z∈L。
(2)群（Group）是一种集合加上一种运算的代数结构，记作 。其中 代表集合， 是定义在该集合上的二元运算。那么，如果这个运算满足以下几个条件，则称 为群。对于矩阵，可以找到一些常见的矩阵群，例如：

李群 是指具有连续性质的群。一般连续群上的运算是无限可微，并解析的（解析比无限可微更强，它还要求任意点邻域的泰勒展开都收敛）。李群，指实数空间上的连续群。常见的李群包括上边提到的GL(n),SO(n),SE(n)以及酉群U(n)等。

（二）旋转矩阵：

关于旋转，在http://blog.csdn.net/piaoxuezhong/article/details/78447822中有关于旋转的表示方法介绍，这里描述下旋转矩阵的性质（百科）：
设 M 是任何维的一般旋转矩阵: 

 

（1）两个向量的点积(内积)在它们都被一个旋转矩阵操作之后保持不变: 

（2）从而得出旋转矩阵的逆矩阵是它的转置矩阵:

 

    这里的    

  

    是单位矩阵。
（3）一个矩阵是旋转矩阵，当且仅当它是正交矩阵并且它的行列式是单位一。正交矩阵的行列式是 ±1；如果行列式是 −1，则它包含了一个反射而不是真旋转矩阵。
（4）旋转矩阵是正交矩阵，如果其列向量形成    的一个正交基，即在任何两个列向量之间的标量积是零(正交性)而每个列向量的大小是单位一。
（5）任何旋转向量可以表示为斜对称矩阵A的指数:

这里的指数是以泰勒级数定义的,而    是以矩阵乘法定义的。A矩阵叫做旋转的“生成元”。旋转矩阵的李代数是它的生成元的代数，它就是斜对称矩阵的代数。生成元可以通过 M 的矩阵对数来找到。

（三）Rodrigues' Rotation Matrix(罗德里格旋转矩阵)

3D旋转可以归结成按照某个单位向量进行某个角度的旋转。已知某个旋转时，可以推导出对应的旋转矩阵。该过程用罗德里格公式求出。
1.旋转角度
旋转前向量为P, 旋转后变为Q。根据点积定义： 

可推出P，Q之间的夹角：

matlab实例:

%假设P,Q为两个向量
P=norm(P); %归一化 
Q=norm(Q);
costheta=P*Q'

2. 旋转轴
旋转角所在的平面为向量P和Q所构成的平面，而旋转轴垂直该平面。假定旋转前向量为a(a1, a2, a3)， 旋转后向量为b（b1, b2, b3)。由叉乘定义得旋转轴c(c1, c2, c3)为：

matlab实例:

%假设P,Q为两个向量，他们的平面法向量为：  
normal=cross(P,Q);  
%归一化  
normal=norm(normal);  

3. 罗德里格旋转公式(Rodrigues' rotation formula)
设v是一个三维空间向量，k是旋转轴的单位向量，则v在右手螺旋定则意义下绕旋转轴k旋转角度θ得到的向量可以由三个不共面的向量v, k和k×v构成的标架表示：

具体推导参见：https://en.wikipedia.org/wiki/Rodrigues'_rotation_formula

matlab实例:

%参照上面的实例；  
theta=acos(costheta);  
Matrix_Rot=makehgtform('axisrotate',normal,theta); 

参考：
https://en.wikipedia.org/wiki/Rotation_matrix
https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html
http://www.cnblogs.com/gaoxiang12/p/5113334.html
http://www.cnblogs.com/gaoxiang12/p/5137454.html （包含An Invitation to 3D vision章节的翻译）