您的位置: 首页  >  文章  >  三维重建学习(1)：基础知识：旋转矩阵与旋转向量

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

分类: 文章 • 2024-02-14 15:53:22

前言

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

旋转矩阵

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

假设坐标系分别绕着xx轴旋转ϕ\phi角，绕yy轴旋转θ\theta角，绕zz轴旋转ψ\psi角，这里旋转的角度就是我们常说的pitch, roll, yaw。设任意某点在旋转前的坐标系中的坐标是(x,y,z)(x,y,z)，旋转后的坐标是(x′,y′,z′)(x^{'}, y^{'}, z^{'})。

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

  • 绕X轴旋转φ角（PITCH）：

Rx=[1000cos⁡ϕ−sin⁡ϕ0sin⁡ϕcos⁡ϕ]R_x=\begin{bmatrix}1 & 0 & 0 \\ 0 & \cos{\phi} & -\sin{\phi} \\ 0 & \sin{\phi} & \cos{\phi} \end{bmatrix}

  • 绕Y轴旋转θ角（ROLL）：

Ry=[cos⁡θ0sin⁡θ010−sin⁡θ0cos⁡θ]R_y=\begin{bmatrix} \cos{\theta} & 0 & \sin{\theta} \\ 0 & 1 & 0 \\ - \sin{\theta} & 0 & \cos{\theta} \end{bmatrix}

  • 绕Z轴旋转ψ角（YAW）：

Rz=[cos⁡ψ−sin⁡ψ0sin⁡ψcos⁡ψ0001]R_z=\begin{bmatrix} \cos{\psi} & -\sin{\psi} & 0 \\ \sin{\psi} & \cos{\psi} & 0 \\ 0 & 0 & 1 \end{bmatrix}

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

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

R=Rx∗Ry∗RzR=R_x * R_y *R_z

旋转向量

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

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

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

对于任意向量a⃗(a1,a2,a3)\vec{a}(a_1,a_2,a_3),b⃗(b1,b2,b3)\vec{b}(b_1,b_2,b_3)，c⃗(c1,c2,c3)\vec{c}(c_1,c_2,c_3)，有如下公式成立：

a⃗×(b⃗×c⃗)=(a⃗⋅c⃗)b⃗−(a⃗⋅b⃗)c⃗\vec{a} \times (\vec{b} \times \vec{c}) = (\vec{a} \cdot \vec{c})\vec{b} - (\vec{a} \cdot \vec{b}) \vec{c}

证明如下：

设d⃗=b⃗×c⃗\vec{d} = \vec{b} \times \vec{c}，且可表示为d⃗(d1,d2,d3)\vec{d}(d_1,d_2,d_3)。

则有：d⃗=b⃗×c⃗=[i⃗j⃗k⃗b1b2b3c1c2c3]=i⃗(b2c3−b3c2)−j⃗(b1c3−b3c1)+k⃗(b1c2−b2c1)\vec{d} = \vec{b} \times \vec{c}=\begin{bmatrix} \vec{i} & \vec{j} & \vec{k} \\ b_1 & b_2 & b_3 \\ c_1 & c_2 & c_3 \end{bmatrix}\\=\vec{i}(b_2c_3-b_3c_2)-\vec{j}(b_1c_3-b_3c_1)+\vec{k}(b_1c_2-b_2c_1)

对应地就有：d1=b2c3−b3c2d_1=b_2c_3-b_3c_2、d2=b3c1−b1c3d_2=b_3c_1-b_1c_3、d3=b1c2−b2c1d_3=b_1c_2-b_2c_1。

再设e⃗=a⃗×d⃗=a⃗×(b⃗×c⃗)\vec{e} = \vec{a} \times \vec{d} = \vec{a} \times (\vec{b} \times \vec{c})，且可表示为e⃗(e1,e2,e3)\vec{e}(e_1,e_2,e_3)。

那么：e⃗=a⃗×d⃗=[i⃗j⃗k⃗a1a2a3d1d2d3]=i⃗(a2d3−a3d2)−j⃗(a1d3−a3d1)+k⃗(a1d2−a2d1)\vec{e} = \vec{a} \times \vec{d}=\begin{bmatrix} \vec{i} & \vec{j} & \vec{k} \\ a_1 & a_2 & a_3 \\ d_1 & d_2 & d_3 \end{bmatrix}\\=\vec{i}(a_2d_3-a_3d_2)-\vec{j}(a_1d_3-a_3d_1)+\vec{k}(a_1d_2-a_2d_1)

对应地得到得到：e1=a2d3−a3d2e_1=a_2d_3-a_3d_2、e2=a1d3−a3d1e_2=a_1d_3-a_3d_1、e3=a1d2−a2d1e_3=a_1d_2-a_2d_1

对于e1e_1，代入前面推导出来的d2d_2、d3d_3：

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⃗)e_1=a_2d_3-a_3d_2=a_2(b_1c_2-b_2c_1)-a_3(b_3c_1-b_1c_3) \\ =b_1 (a_2c_2 + a_3c_3) - c_1 (a_3b_3 + a_2b_2) \\ = b_1 (\vec{a} \cdot \vec{c} - a_1c_1) - c_1 (\vec{a} \cdot \vec{b} - a_1b_1) \\ = b_1 (\vec{a} \cdot \vec{c}) - c_1 (\vec{a} \cdot \vec{b})

同理可得：

e2=b2(a⃗⋅c⃗)−c2(a⃗⋅b⃗)e_2 = b_2 (\vec{a} \cdot \vec{c}) - c_2 (\vec{a} \cdot \vec{b})

e3=b3(a⃗⋅c⃗)−c3(a⃗⋅b⃗)e_3 = b_3 (\vec{a} \cdot \vec{c}) - c_3 (\vec{a} \cdot \vec{b})

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

e⃗=b⃗(a⃗⋅c⃗)−c⃗(a⃗⋅b⃗)\vec{e} = \vec{b} (\vec{a} \cdot \vec{c}) - \vec{c} (\vec{a} \cdot \vec{b})

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

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

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

很明显：v⊥⃗\vec{v_{\bot}}与k⃗\vec{k}相互垂直，v∥⃗\vec{v_{\parallel}}与k⃗\vec{k}共线（平行），且还有v⃗=v⊥⃗+v∥⃗\vec{v}=\vec{v_{\bot}}+\vec{v_{\parallel}}。

因为v∥⃗\vec{v_{\parallel}}与k⃗\vec{k}共线，且k⃗\vec{k}是一个单位向量，所以：

v∥⃗=∥v∥⃗∥⋅k⃗=v⃗⋅k⃗∥k⃗∥⋅k⃗=(v⃗⋅k⃗)⋅k⃗\vec{v_{\parallel}}=\|\vec{v_{\parallel}}\| \cdot \vec{k}= \frac{\vec{v} \cdot \vec{k}}{\| \vec{k} \|} \cdot \vec{k} = (\vec{v} \cdot \vec{k}) \cdot \vec{k}

接下来求v⊥⃗\vec{v_{\bot}}，由我们前面推导的向量的拉格朗日定理：a⃗×(b⃗×c⃗)=(a⃗⋅c⃗)b⃗−(a⃗⋅b⃗)c⃗\vec{a} \times (\vec{b} \times \vec{c}) = (\vec{a} \cdot \vec{c})\vec{b} - (\vec{a} \cdot \vec{b}) \vec{c},有：

k⃗×(k⃗×v⃗)=(k⃗⋅v⃗)⋅k⃗−(k⃗⋅k⃗)⋅v⃗=(k⃗⋅v⃗)⋅k⃗−v⃗\vec{k} \times (\vec{k} \times \vec{v}) = (\vec{k} \cdot \vec{v}) \cdot \vec{k} - (\vec{k} \cdot \vec{k}) \cdot \vec{v}=(\vec{k} \cdot \vec{v}) \cdot \vec{k} - \vec{v}

然后，我们就可以推出：

v⊥⃗=v⃗−v∥⃗=−k⃗×(k⃗×v⃗)\vec{v_{\bot}}=\vec{v} - \vec{v_{\parallel}} = -\vec{k} \times (\vec{k} \times \vec{v})

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

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

接下来看vrot,⊥⃗\vec{v_{rot, \bot}}的变化：

首先，可以肯定的是旋转过程中矢量的模长不变，只会改变方向，所以：∥vrot,⊥⃗∥=∥v⊥⃗∥\| \vec{v_{rot, \bot}} \| = \| \vec{v_{\bot}} \|。

我们可以由v⊥⃗\vec{v_{\bot}}和k⃗\vec{k}方向的投影，组合得到vrot,⊥⃗\vec{v_{rot, \bot}}：

vrot,⊥⃗=v⊥⃗cos⁡θ+∥v⊥⃗∥sin⁡θw⃗∥w⃗∥\vec{v_{rot, \bot}}=\vec{v_{\bot}}\cos\theta + \| \vec{v_{\bot}} \| \sin\theta \frac{\vec{w}}{\|\vec{w}\|}

前面有：w⃗=k⃗×v⃗\vec{w}=\vec{k} \times \vec{v}可以看作是v⊥⃗\vec{v_{\bot}}绕着k⃗\vec{k}逆时针旋转90度得到的。那么有：∥v⊥⃗∥=∥w⃗∥\| \vec{v_{\bot}} \| = \|\vec{w}\|。

上式可以化简为：

vrot,⊥⃗=v⊥⃗cos⁡θ+sin⁡θw⃗=v⊥⃗cos⁡θ+sin⁡θ(k⃗×v⃗)\vec{v_{rot, \bot}} = \vec{v_{\bot}}\cos\theta + \sin\theta \vec{w}= \vec{v_{\bot}}\cos\theta + \sin\theta (\vec{k} \times \vec{v})

将vrot,⊥⃗\vec{v_{rot, \bot}}和vrot,∥⃗\vec{v_{rot, \parallel}}相加，即可得到vrot⃗\vec{v_{rot}}：

vrot⃗=vrot,⊥⃗+vrot,∥⃗=v∥⃗+v⊥⃗cos⁡θ+sin⁡θ(k⃗×v⃗)=v∥⃗+(v⃗−v∥⃗)cos⁡θ+sin⁡θ(k⃗×v⃗)=v⃗cos⁡θ+(1−cos⁡θ)v∥⃗+sin⁡θ(k⃗×v⃗) \begin{aligned} \vec{v_{rot}} &= \vec{v_{rot, \bot}} + \vec{v_{rot, \parallel}} \\ &= \vec{v_{\parallel}} + \vec{v_{\bot}}\cos\theta + \sin\theta (\vec{k} \times \vec{v}) \\ &= \vec{v_{\parallel}} + (\vec{v} - \vec{v_{\parallel}}) \cos\theta +\sin\theta (\vec{k} \times \vec{v}) \\ &= \vec{v}\cos\theta + (1-\cos\theta) \vec{v_{\parallel}} + \sin\theta (\vec{k} \times \vec{v}) \end{aligned}

前面推出来了：v∥⃗=∥v∥⃗∥⋅k⃗=v⃗⋅k⃗∥k⃗∥⋅k⃗=(v⃗⋅k⃗)⋅k⃗\vec{v_{\parallel}}=\|\vec{v_{\parallel}}\| \cdot \vec{k}= \frac{\vec{v} \cdot \vec{k}}{\| \vec{k} \|} \cdot \vec{k} = (\vec{v} \cdot \vec{k}) \cdot \vec{k}，直接把那个结论代入这里。

vrot⃗=v⃗cos⁡θ+(1−cos⁡θ)(v⃗⋅k⃗)⋅k⃗+sin⁡θ(k⃗×v⃗)\begin{aligned} \vec{v_{rot}} &= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} \cdot \vec{k}) \cdot \vec{k} + \sin\theta (\vec{k} \times \vec{v}) \end{aligned}

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

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

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

[(k⃗×v⃗)x(k⃗×v⃗)y(k⃗×v⃗)z]=[kyvz−kzvykzvx−kxvzkxvy−kyvx]=[0−kzkykz0−kx−kykx0]⋅[vxvyvz]\begin{bmatrix} (\vec{k} \times \vec{v})_x \\ (\vec{k} \times \vec{v})_y \\ (\vec{k} \times \vec{v})_z \end{bmatrix} = \begin{bmatrix}k_yv_z-k_zv_y \\ k_zv_x-k_xv_z \\ k_xv_y - k_yv_x \end{bmatrix} = \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x &0 \end{bmatrix} \cdot \begin{bmatrix} v_x \\ v_y \\ v_z\end{bmatrix}

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

K=[0−kzkykz0−kx−kykx0]K=\begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x &0 \end{bmatrix}

那么有：Kv⃗=k⃗×v⃗K\vec{v}=\vec{k} \times \vec{v}

还有：K(Kv⃗)=K2v⃗=K(k⃗×v⃗)=k⃗×(Kv⃗)=k⃗×(k⃗×v⃗)K(K\vec{v})=K^2\vec{v}=K(\vec{k} \times \vec{v}) = \vec{k} \times (K\vec{v}) = \vec{k} \times (\vec{k} \times \vec{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⃗\begin{aligned} \vec{v_{rot}} &= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} \cdot \vec{k}) \cdot \vec{k} + \sin\theta (\vec{k} \times \vec{v}) \\ &= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} + \vec{k} \times (\vec{k} \times \vec{v})) + \sin\theta (\vec{k} \times \vec{v}) \\ &= \vec{v} + (1-\cos\theta)K^2\vec{v} + sin\theta K \vec{v} \end{aligned}

给出旋转矩阵RR有：vrot⃗=Rv⃗\vec{v_{rot}} = R \vec{v}

那么消去v⃗\vec{v}：$R = I + (1-\cos\theta)K^2 + sin\theta K $

Rodrigue旋转向量公式

已知单位向量k⃗=(kx,ky,kz)\vec{k}=(k_x,k_y,k_z)，向量v⃗\vec{v}绕着它旋转θ\theta角。旋转后的向量vrot⃗\vec{v_{rot}}可以通过如下公式求得：

vrot⃗=v⃗cos⁡θ+(1−cos⁡θ)(v⃗⋅k⃗)⋅k⃗+sin⁡θ(k⃗×v⃗)\begin{aligned} \vec{v_{rot}} &= \vec{v}\cos\theta + (1-\cos\theta) (\vec{v} \cdot \vec{k}) \cdot \vec{k} + \sin\theta (\vec{k} \times \vec{v}) \end{aligned}

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

vrot⃗=Rv⃗=(I⋅cos⁡θ+(1−cos⁡θ)k⃗⋅k⃗+sin⁡θK)v⃗\begin{aligned} \vec{v_{rot}} &= R \vec{v} \\ &= (I \cdot \cos\theta + (1-\cos\theta) \vec{k} \cdot \vec{k} + \sin\theta K)\vec{v} \end{aligned}

其中：

K=[0−kzkykz0−kx−kykx0]K=\begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x &0 \end{bmatrix}

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

对于旋转向量r⃗=(rx,ry,rz)\vec{r} = (r_x, r_y, r_z)：
三维重建学习(1)：基础知识：旋转矩阵与旋转向量

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

反变换也可以很容易求出：
三维重建学习(1)：基础知识：旋转矩阵与旋转向量

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

当然还有一种写法：

vrot⃗=Rv⃗=I⋅v⃗+(1−cos⁡θ)K2⋅v⃗+sinθK⋅v⃗\vec{v_{rot}} = R\vec{v} = I \cdot \vec{v} + (1-\cos\theta)K^2 \cdot \vec{v} + sin\theta K \cdot \vec{v}

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

R=I+(1−cos⁡θ)K2+sinθKR = I + (1-\cos\theta)K^2 + sin\theta K

其中：

K=[0−kzkykz0−kx−kykx0]K=\begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x &0 \end{bmatrix}

参考链接：

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

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

  3. Rodrigues旋转向量维基百科

  4. http://blog.****.net/tl_tj/article/details/47006007

  5. http://wiki.opencv.org.cn/index.php/Cv照相机定标和三维重建#Rodrigues2

    ​

相关推荐