三维重建学习(1)：基础知识：旋转矩阵与旋转向量

前言

由于摄像机标定中会使用到旋转矩阵以及旋转向量的知识，所以就整理了一下有关与这一部分基础知识的笔记，并进行详细的数学推导。

旋转矩阵

假设坐标系分别绕着x轴旋转ϕ角，绕y轴旋转θ角，绕z轴旋转ψ角，这里旋转的角度就是我们常说的pitch, roll, yaw。设任意某点在旋转前的坐标系中的坐标是(x,y,z)，旋转后的坐标是(x‘,y‘,z‘)。

我们可以很容易地推导出这三种情况下的旋转矩阵：

绕X轴旋转φ角（PITCH）：

Rx=⎡⎣⎢1000cosϕsinϕ0−sinϕcosϕ⎤⎦⎥

绕Y轴旋转θ角（ROLL）：

Ry=⎡⎣⎢cosθ0−sinθ010sinθ0cosθ⎤⎦⎥

绕Z轴旋转ψ角（YAW）：

Rz=⎡⎣⎢cosψsinψ0−sinψcosψ0001⎤⎦⎥

详细步骤我就不列出来了，我以前的博客中有详细的推导步骤，虽然使用场景不同，但是概念都是一样的。（四旋翼姿态解算——基础理论及推导)

如果已经上面的三个角度ϕ、θ、ψ，，我们可以通过将每一步的旋转矩阵相乘得到整个旋转矩阵。

R=Rx∗Ry∗Rz

旋转向量

相机标定中表示旋转的方式，除了使用前面介绍的旋转矩阵之外，还可以使用旋转向量来表示。

我在网上搜索相关资料时，发现大多数都是直接给出了Rodrigue旋转向量的公式，而没有推导过程。如果只是拿来用，那肯定是没问题的，但是不把它的公式推导一遍，心里总是有点不踏实。（好吧，我知道我有点废话）下面我会给出我自己的证明步骤，在最后会给出公式。

补充概念：向量的拉格朗日公式

对于任意向量a⃗ (a1,a2,a3),b⃗ (b1,b2,b3)，c⃗ (c1,c2,c3)，有如下公式成立：

a⃗ ×(b⃗ ×c⃗ )=(a⃗ ⋅c⃗ )b⃗ −(a⃗ ⋅b⃗ )c⃗

证明如下：

设d⃗ =b⃗ ×c⃗ ，且可表示为d⃗ (d1,d2,d3)。

则有：d⃗ =b⃗ ×c⃗ =⎡⎣⎢i⃗ b1c1j⃗ b2c2k⃗ b3c3⎤⎦⎥=i⃗ (b2c3−b3c2)−j⃗ (b1c3−b3c1)+k⃗ (b1c2−b2c1)

对应地就有：d1=b2c3−b3c2、d2=b3c1−b1c3、d3=b1c2−b2c1。

再设e⃗ =a⃗ ×d⃗ =a⃗ ×(b⃗ ×c⃗ )，且可表示为e⃗ (e1,e2,e3)。

那么：e⃗ =a⃗ ×d⃗ =⎡⎣⎢i⃗ a1d1j⃗ a2d2k⃗ a3d3⎤⎦⎥=i⃗ (a2d3−a3d2)−j⃗ (a1d3−a3d1)+k⃗ (a1d2−a2d1)

对应地得到得到：e1=a2d3−a3d2、e2=a1d3−a3d1、e3=a1d2−a2d1

对于e1，代入前面推导出来的d2、d3：

e1=a2d3−a3d2=a2(b1c2−b2c1)−a3(b3c1−b1c3)=b1(a2c2+a3c3)−c1(a3b3+a2b2)=b1(a⃗ ⋅c⃗ −a1c1)−c1(a⃗ ⋅b⃗ −a1b1)=b1(a⃗ ⋅c⃗ )−c1(a⃗ ⋅b⃗ )

同理可得：

e2=b2(a⃗ ⋅c⃗ )−c2(a⃗ ⋅b⃗ )

e3=b3(a⃗ ⋅c⃗ )−c3(a⃗ ⋅b⃗ )

把这三个式子合并，表示成向量形式：

e⃗ =b⃗ (a⃗ ⋅c⃗ )−c⃗ (a⃗ ⋅b⃗ )

证明完毕，后面我们还会用到这个公式。



上图摘自维基百科。假设我们在任意空间中有任意一个向量v⃗ ，k⃗ 是某个单位向量。现在，我们将向量v⃗ 以单位向量k⃗ 为轴，旋转任意角度θ，得到旋转后的向量vrot→如图中所示。在图中，我们将v⃗ 关于k⃗ 做正交分解（高中物理），会得到两个新的向量：v⊥→和v∥→。

很明显：v⊥→与k⃗ 相互垂直，v∥→与k⃗ 共线（平行），且还有v⃗ =v⊥→+v∥→。

因为v∥→与k⃗ 共线，且k⃗ 是一个单位向量，所以：

v∥→=∥v∥→∥⋅k⃗ =v⃗ ⋅k⃗ ∥k⃗ ∥⋅k⃗ =(v⃗ ⋅k⃗ )⋅k⃗

接下来求v⊥→，由我们前面推导的向量的拉格朗日定理：a⃗ ×(b⃗ ×c⃗ )=(a⃗ ⋅c⃗ )b⃗ −(a⃗ ⋅b⃗ )c⃗ ,有：

k⃗ ×(k⃗ ×v⃗ )=(k⃗ ⋅v⃗ )⋅k⃗ −(k⃗ ⋅k⃗ )⋅v⃗ =(k⃗ ⋅v⃗ )⋅k⃗ −v⃗

然后，我们就可以推出：

v⊥→=v⃗ −v∥→=−k⃗ ×(k⃗ ×v⃗ )

我们可以根据前面给出的示意图来直观地理解这些式子：k⃗ ×v⃗ 可以看作是v⊥→绕着k⃗ 逆时针旋转90度得到的，即图中的w⃗ =k⃗ ×v⃗ ；对于k⃗ ×(k⃗ ×v⃗ )=k⃗ ×w⃗ ，在图中观察发现，这个式子的结果其实就是v⊥→绕着k⃗ 逆时针旋转180度得到的结果，共线且夹角为180度，正好对应推导出的结果：v⊥→=−k⃗ ×(k⃗ ×v⃗ )。

还是根据这幅图来看，旋转过程中v∥→的大小始终不变，因为它是v⃗ 在k⃗ 方向上的投影，就在旋转轴上，自然不会变。所以有：vrot,∥→=v∥→。

接下来看vrot,⊥→的变化：

首先，可以肯定的是旋转过程中矢量的模长不变，只会改变方向，所以：∥vrot,⊥→∥=∥v⊥→∥。

我们可以由v⊥→和k⃗ 方向的投影，组合得到vrot,⊥→：

vrot,⊥→=v⊥→cosθ+∥v⊥→∥sinθw⃗ ∥w⃗ ∥

前面有：w⃗ =k⃗ ×v⃗ 可以看作是v⊥→绕着k⃗ 逆时针旋转90度得到的。那么有：∥v⊥→∥=∥w⃗ ∥。

上式可以化简为：

vrot,⊥→=v⊥→cosθ+sinθw⃗ =v⊥→cosθ+sinθ(k⃗ ×v⃗ )

将vrot,⊥→和vrot,∥→相加，即可得到vrot→：

vrot→=vrot,⊥→+vrot,∥→=v∥→+v⊥→cosθ+sinθ(k⃗ ×v⃗ )=v∥→+(v⃗ −v∥→)cosθ+sinθ(k⃗ ×v⃗ )=v⃗ cosθ+(1−cosθ)v∥→+sinθ(k⃗ ×v⃗ )

前面推出来了：v∥→=∥v∥→∥⋅k⃗ =v⃗ ⋅k⃗ ∥k⃗ ∥⋅k⃗ =(v⃗ ⋅k⃗ )⋅k⃗ ，直接把那个结论代入这里。

vrot→=v⃗ cosθ+(1−cosθ)(v⃗ ⋅k⃗ )⋅k⃗ +sinθ(k⃗ ×v⃗ )

现在我们已经推导出了旋转向量公示了，我们可以套用这个公式来表示任意旋转。

接下来要进一步整理成矩阵形式：

对于k⃗ ×v⃗ ，用矩阵形式来表示：（无非就是向量叉乘，拆成三个轴上的分量来表示）

⎡⎣⎢⎢⎢(k⃗ ×v⃗ )x(k⃗ ×v⃗ )y(k⃗ ×v⃗ )z⎤⎦⎥⎥⎥=⎡⎣⎢kyvz−kzvykzvx−kxvzkxvy−kyvx⎤⎦⎥=⎡⎣⎢0kz−ky−kz0kxky−kx0⎤⎦⎥⋅⎡⎣⎢vxvyvz⎤⎦⎥

使用一个大写的K来表示：

K=⎡⎣⎢0kz−ky−kz0kxky−kx0⎤⎦⎥

那么有：Kv⃗ =k⃗ ×v⃗

还有：K(Kv⃗ )=K2v⃗ =K(k⃗ ×v⃗ )=k⃗ ×(Kv⃗ )=k⃗ ×(k⃗ ×v⃗ )

公式可以进一步化简为：

vrot→=v⃗ cosθ+(1−cosθ)(v⃗ ⋅k⃗ )⋅k⃗ +sinθ(k⃗ ×v⃗ )=v⃗ cosθ+(1−cosθ)(v⃗ +k⃗ ×(k⃗ ×v⃗ ))+sinθ(k⃗ ×v⃗ )=v⃗ +(1−cosθ)K2v⃗ +sinθKv⃗

给出旋转矩阵R有：vrot→=Rv⃗

那么消去v⃗ ：R=I+(1−cosθ)K2+sinθK

Rodrigue旋转向量公式

已知单位向量k⃗ =(kx,ky,kz)，向量v⃗ 绕着它旋转θ角。旋转后的向量vrot→可以通过如下公式求得：

vrot→=v⃗ cosθ+(1−cosθ)(v⃗ ⋅k⃗ )⋅k⃗ +sinθ(k⃗ ×v⃗ )

我们可以把v⃗ 提取出来，就得到了：

vrot→=Rv⃗ =(I⋅cosθ+(1−cosθ)k⃗ ⋅k⃗ +sinθK)v⃗

其中：

K=⎡⎣⎢0kz−ky−kz0kxky−kx0⎤⎦⎥

根据上面这个式子，我们可以给出Rodrigue旋转公式：

对于旋转向量r⃗ =(rx,ry,rz)：



θ为向量r⃗ 的模长，而且要保证向量r⃗ 是一个单位向量。

反变换也可以很容易求出：



注：在opencv中有相关的实现（Rodrigues2函数）

当然还有一种写法：

vrot→=Rv⃗ =I⋅v⃗ +(1−cosθ)K2⋅v⃗ +sinθK⋅v⃗

对应的旋转矩阵：（我们可以用这个公式来求旋转矩阵）

R=I+(1−cosθ)K2+sinθK

其中：

K=⎡⎣⎢0kz−ky−kz0kxky−kx0⎤⎦⎥

参考链接：

http://blog.sina.com.cn/s/blog_5fb3f125010100hp.html

https://www.cnblogs.com/xpvincent/archive/2013/02/15/2912836.html

Rodrigues旋转向量维基百科

http://blog.csdn.net/tl_tj/article/details/47006007

http://wiki.opencv.org.cn/index.php/Cv%E7%85%A7%E7%9B%B8%E6%9C%BA%E5%AE%9A%E6%A0%87%E5%92%8C%E4%B8%89%E7%BB%B4%E9%87%8D%E5%BB%BA#Rodrigues2

​