1.相机成像原理

相机成像原理本质上就是点在世界坐标系、相机坐标系、像平面坐标系和像素平面坐标系四个坐标系中间的矩阵变换。
总体图如下：

矩阵关系可写作：
$$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& \frac{U}{2}\\ 0& \frac{1}{dy}& \frac{V}{2}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0_3^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$

1. 世界坐标系——>相机坐标系：
   

上述坐标的变换关系可以写为：
$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0_3^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$$
2. 相机坐标系——>像平面坐标系
相机坐标系中的某点会投影在相机的成像平面上，利用针孔成像原理，空间任意一点$p_c$与图像点$p$之间的关系，$p_c$与相机光心$o_c$的连线为$o_c{p}_c$，与像面的交点p即为空间点$p_c$在图像平面上的投影。

在已知相机焦距$f$的情况下,可得：
$$\frac{x}{x_c}=\frac{y}{y_c}=\frac{f}{z_c}$$
则$p_c$和$p$的关系可以如下：
$$z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]$$
3. 像平面坐标系——>像素坐标系
像素坐标系和图像坐标系都在成像平面上，只是各自的原点和度量单位不一样。图像坐标系的原点为相机光轴与成像平面的交点，通常情况下是成像平面的中点。

由于图像坐标系的单位是mm，属于物理单位，而像素坐标系的单位是pixel，故两单位之间存在下面的变换关系：
$$\begin{array}{ll}1pixel\ast dx=1mm& u与x\\ 1pixel\ast dy=1mm& v与y\end{array}$$
故$(u,v)$与$(x,y)$之间的变换关系如下：
$$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& \frac{U}{2}\\ 0& \frac{1}{dy}& \frac{V}{2}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$

2 旋转平移

2.1 李群和李代数

2.2 罗德里格斯公式

1. 旋转的分解：
   
   由上图可知：
   $$\overrightarrow{v}={\overrightarrow{v}}_{||}+{\overrightarrow{v}}_{\perp }$$
   对与${\overrightarrow{v}}_{||}$来说，因为其与$\overrightarrow{u}$平行，所以其旋转仍然为其本身。
   对于${\overrightarrow{v}}_{\perp }$来说，看下图：
   

其旋转为：
$$\begin{array}{l}\overrightarrow{w}=\overrightarrow{u}\times {\overrightarrow{v}}_{\perp }\\ \begin{array}{rl}{\overrightarrow{v}}_{\perp }^{{}^{\prime }}& ={\overrightarrow{v}}_v+{\overrightarrow{v}}_w^{{}^{\prime }}\\ & =cos\theta {\overrightarrow{v}}_{\perp }+sin\theta \overrightarrow{w}\\ & =cos\theta {\overrightarrow{v}}_{\perp }+sin\theta (\overrightarrow{u}\times {\overrightarrow{v}}_{\perp })\end{array}\end{array}$$

也就是说${\overrightarrow{v}}_{\perp }$旋转$\theta$角度后的结果如下：
$${\overrightarrow{v}}_{\perp }^{{}^{\prime }}=cos\theta {\overrightarrow{v}}_{\perp }+sin\theta (\overrightarrow{u}\times {\overrightarrow{v}}_{\perp })$$

又因为：
$$\begin{array}{rl}& {\overrightarrow{v}}_{||}^{{}^{\prime }}={\overrightarrow{v}}_{||}=(\overrightarrow{u}\cdot \overrightarrow{v})\overrightarrow{u}\\ & {\overrightarrow{v}}_{\perp }=\overrightarrow{v}-(\overrightarrow{u}\cdot \overrightarrow{v})\overrightarrow{u}\\ & \overrightarrow{u}\times {\overrightarrow{v}}_{\perp }=\overrightarrow{u}\times \overrightarrow{v}\end{array}$$
通过上述可得：
$$\begin{array}{l}\begin{array}{rl}{\mathbf{v}}^{{}^{\prime }}& ={\mathbf{v}}_{||}^{{}^{\prime }}+{\mathbf{v}}_{\perp }^{{}^{\prime }}\\ & =(\mathbf{u}\cdot \mathbf{v})\mathbf{u}+cos\theta (\mathbf{v}-(\mathbf{u}\cdot \mathbf{v})\mathbf{u})+sin\theta (\mathbf{u}\times \mathbf{v})\\ & =cos\theta \mathbf{v}+(1-cos\theta )(\mathbf{u}\cdot \mathbf{v})\mathbf{u}+sin\theta (\mathbf{u}\times \mathbf{v})\\ & =v+(1-cos\theta )\ast \mathbf{u}\times (\mathbf{u}\cdot \mathbf{v})+sin\theta (\mathbf{u}\times \mathbf{v})\end{array}\end{array}$$

上述公式称作罗德里格斯公式。

2.3 2D旋转

在进行2D旋转讨论之前，我们首先要了解复数和矩阵之间的关系：

• 复数乘法和矩阵之间的关系：

  $$\begin{array}{l}{z}_1=a+bi\\ z_2=c+di\\ z_1{z}_2=ac-bd+adi+bci=ac-bd+(bc+ad)i\end{array}$$
  如果仔细观察就能发现，复数相乘的结果其实也是一个矩阵与向量相乘的
  结果，也就是说:
  $$z_1{z}_2=ac-bd+(bc+ad)i=\left[\begin{array}{cc}a& -b\\ b& a\end{array}\right]\left[\begin{array}{c}c\\ d\end{array}\right]$$

  $\left[\begin{array}{c}c\\ d\end{array}\right]$为z_2的向量形式，$\left[\begin{array}{cc}a& -b\\ b& a\end{array}\right]$为z_1的矩阵形式。接下来如果我们将$z_1$和$z_2$用矩阵形式表示并相乘的话，可以得到下式：
  $$z_1{z}_2=\left[\begin{array}{cc}a& -b\\ b& a\end{array}\right]\left[\begin{array}{cc}c& -d\\ d& c\end{array}\right]=\left[\begin{array}{cc}ac-bd& -(bc+ad)\\ bc+ad& ac-bd\end{array}\right]$$
  可以观察到结果$\left[\begin{array}{cc}ac-bd& -(bc+ad)\\ bc+ad& ac-bd\end{array}\right]$为$ac-bd+(bc+ad)i$的矩阵形式。
  所以我们可以所以矩阵的乘法来表示复数的乘法。
  下面为1和i的矩阵形式：
  $$\begin{array}{l}1=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\\ i=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]\end{array}$$

• 复数相乘和2D旋转的联系

  通过上面的推导我们可以发现，复数相乘和矩阵相乘是可以等价的，那么也就可以将复数与2D旋转联系到一起。
  
  观察上图我们可得：
  $$\begin{array}{rl}z& =a+bi=\left[\begin{array}{cc}a& -b\\ b& a\end{array}\right]\\ & =\sqrt{a^2+b^2}\left[\begin{array}{cc}\frac{a}{\sqrt{a^2+b^2}}& \frac{-b}{\sqrt{a^2+b^2}}\\ \frac{b}{\sqrt{a^2+b^2}}& \frac{a}{\sqrt{a^2+b^2}}\end{array}\right]\\ & =\sqrt{a^2+b^2}\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\\ & =\left[\begin{array}{cc}\sqrt{a^2+b^2}& 0\\ 0& \sqrt{a^2+b^2}\end{array}\right]\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\\ & =\left[\begin{array}{cc}||z||& 0\\ 0& ||z||\end{array}\right]\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\end{array}$$

  我们将原本的矩阵变形为了两个变换矩阵的复合，其中左边的$\left[\begin{array}{cc}||z||& 0\\ 0& ||z||\end{array}\right]$
  是缩放矩阵，而右边的$\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]$则是我们熟悉的2D旋转矩阵。

  接下来我们测试一下，旋转矩阵对1和i的影响，也就是$\left[\begin{array}{c}1\\ 0\end{array}\right]$和$\left[\begin{array}{c}0\\ 1\end{array}\right]$。

  $$\begin{gathered} \begin{array}{l}\left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}1\\ 0\end{array}\right]=\left[\begin{array}{c}cos\theta \\ sin\theta \end{array}\right]\end{array} \\ ------------- \\ \left[\begin{array}{cc}cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]=\left[\begin{array}{c}-sin\theta \\ cos\theta \end{array}\right] \end{gathered}$$

  旋转图如下：
  

  也就是说，单位旋转矩阵会将被旋转向量逆时针旋转$\theta$。

  如果不是单位旋转矩阵的话，会对被旋转向量进行一个尺度的缩放。

  总结上述推导，可以得出：复数的相乘其实是旋转与缩放变换的复合．如果有一个复数 $z=a+bi$，
  那么 $z$ 与任意一个复数$c$相乘都会将$c$逆时针旋转$\theta =atan2(b,a)$，并将
  其缩放$||z||=\sqrt{a^2+b^2}$倍。

• 复数的极坐标形式：

  根据欧拉公式可知：
  $$cos\theta +isin\theta =e^{i\theta }$$
  也就是说：
  $$z=a+bi==||z||(cos\theta +isin\theta )=||z||e^{i\theta }$$
  如果我们定义$r=||z||$，我们就得到了复数的极坐标形式：
  $$z=r{e}^{i\theta }$$
  现在复数的定义就与实部与虚部的两个分量$a$,$b$无关了，我们可以使用一
  个缩放因子$r$ 和旋转角度$\theta$的形式来定义任意一个复数，而且它旋转与缩放的性质仍然存在。
  $$v^{{}^{\prime }}=r{e}^{i\theta }v$$

2.4 3D旋转

加粗的符号为向量

• 1.欧拉角

• 2.轴角式

  通常情况下，如果我们说绕着一个向量 u 旋转，我们其实指的是绕着$u$所指的方向进行旋转。向量是同时具有大小和方向的量，但是在旋转中，其大小并不重要。我们可以说绕着$u_1={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$这个轴进行旋转，也可以说绕着$u_2={\left[\begin{array}{ccc}0& 0& 2\end{array}\right]}^T$旋转。即使这两个向量完全不同，但是它们指向的都是同一个方向（即 轴的方向）

  轴角式的旋转和罗德里格斯公式的推导一致。

• 3.四元数

  • 四元数的定义

    四元数的定义和复数非常类似，唯一的区别就是四元数一共有三个虚部，而复数只有一个。所有的四元数$q\in H$($H$代表四元数的发现者 William Rowan Hamilton)都可以。
    $$q=a+bi+cj+dk$$
    其中
    $$i^2=j^2=k^2=ijk=-1$$
    与复数类似，四元数可以写作向量形式：
    $$q=\left[\begin{array}{c}a\\ b\\ c\\ d\end{array}\right]$$

    在描述四元数时，我们经常将四元数写作：
    $$q=[s,\mathbf{v}]$$
    将实部和虚部分开，表示为标量和向量的有序对形式。

  • 四元数的性质

    1. 模长

       和复数类似，四元数的模长（范数）可以表示为：
       $$||q||=\sqrt{a^2+b^2+c^2+d^2}$$

    2. 四元数乘法和矩阵之间的联系

       四元数不满足乘法交换律。和复数类似，四元数之间的乘法也可以写作矩阵的形式：
       $$\begin{array}{rl}{q}_1{q}_2& =(a+bi+cj+dk)(e+fi+gj+hk)\\ & =ae+afi+agj+ahk+bei+bf{i}^2\\ & +bgij+bhik+cej+cfji+cgj2\\ & +chjk+dek+dfki+dgkj+dhk2\end{array}$$
       化简得：
       $$\begin{array}{rl}{q}_1{q}_2=& (ae-bf-cg-dh)+\\ & (be+af-dg+ch)i+\\ & (ce+df+ag-bh)j+\\ & (de-cf+bg+ah)k\end{array}$$
       写成矩阵*向量的形式为：
       $$q_1{q}_2=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& -d& c\\ c& d& a& -b\\ d& -c& b& a/\end{array}\right]\left[\begin{array}{c}e\\ f\\ g\\ h\end{array}\right]$$
       由于四元数不满足乘法交换律，故$q_2{q}_1$如果将$q_1$表示成矩阵的形式为不同的形式：
       $$q_2{q}_1=\left[\begin{array}{cccc}a& -b& -c& -d\\ b& a& d& -c\\ c& -d& a& b\\ d& c& -b& a\end{array}\right]\left[\begin{array}{c}e\\ f\\ g\\ h\end{array}\right]$$

    3. Graßmann积
       假设$q_1=[a,b,c,d{]}^T=\left[\begin{array}{c}a\\ \mathbf{v}\end{array}\right]$，$q_2=[e,f,g,h{]}^T=\left[\begin{array}{c}e\\ \mathbf{u}\end{array}\right]$,
       那么根据上面四元数的乘积可以得出：
       $$q_1{q}_2=\left[\begin{array}{c}ae-\mathbf{v}\cdot \mathbf{u}\\ a\mathbf{u}+e\mathbf{v}+\mathbf{v}\times \mathbf{u}\end{array}\right]$$
       其中：
       $$\begin{array}{rl}\mathbf{v}\cdot \mathbf{u}& =bf+cg+dh\\ \mathbf{v}\times \mathbf{u}& =(ch-dg)i-(bh-df)j+(bg-cf)k\end{array}$$

    4. 纯四元数乘积

       $$vu=\left[\begin{array}{c}0-\mathbf{v}\cdot \mathbf{u}\\ 0+\mathbf{v}\times \mathbf{u}\end{array}\right]=\left[\begin{array}{c}-\mathbf{v}\cdot \mathbf{u}\\ \mathbf{v}\times \mathbf{u}\end{array}\right]$$

    5. 共轭和逆
       共轭：

       $$\begin{gathered} q=a+bi+cj+dk \\ {q}^{\ast }=a-bi-cj-dk \\ q=\left[\begin{array}{c}s\\ \mathbf{v}\end{array}\right] \\ {q}^{\ast }=\left[\begin{array}{c}s\\ -\mathbf{v}\end{array}\right] \end{gathered}$$
       共轭四元数有个重要的性质：
       $$q{q}^{\ast }=\left[\begin{array}{c}{s}^2+\mathbf{v}\cdot \mathbf{v}\\ 0\end{array}\right]$$
       也就是说结果为一个实数：$q{q}^{\ast }=s^2+x^2+y^2+z^2=||q|{|}^2$
       同理可以得到：$q^{\ast }q=s^2+x^2+y^2+z^2=||q|{|}^2=q{q}^{\ast }$

       也就是说这个特殊的乘法是遵守交换律的。而且可以通过这个性质获得逆。

       逆：
       $$q^{-1}=\frac{q^{\ast }}{||q|{|}^2}$$
       如果$||q|{|}^2$等于1，那么：$q^{-1}=q^{\ast }$

  进行推导前，先说出四元数旋转的结论：
  任何向量$v$沿着以单位向量定义的旋转轴$u$旋转$\theta$度之后的$v^{{}^{\prime }}$可以使用四元数乘法来获得。令$v=\left[\begin{array}{c}0\\ \mathbf{v}\end{array}\right]$,$q=\left[\begin{array}{c}cos(\frac{1}{2}\theta )\\ sin(\frac{1}{2}\theta )\mathbf{u}\end{array}\right]$,那么：
  $$v^{{}^{\prime }}=qvq\ast =qv{q}^{-1}$$
  注意这里仅仅使用向量形式表示四元数而已，下面的运算是四元数的运算，不是所谓的向量运算。

  接下来进行推导：
  首先$v=\left[\begin{array}{c}0\\ \mathbf{v}\end{array}\right]$，$u=\left[\begin{array}{c}0\\ \mathbf{u}\end{array}\right]$，$v=v_{\perp }+v_{||}=\left[\begin{array}{c}0\\ {\mathbf{v}}_{\perp }\end{array}\right]+\left[\begin{array}{c}0\\ {\mathbf{v}}_{||}\end{array}\right]$

  第一步对$v_{||}$旋转：
  $v_{||}$旋转不变。

  第二步对$v_{\perp }$旋转：
  在前面轴角式的推到中，我们得到：
  $${\mathbf{v}}_{\perp }^{{}^{\prime }}=cos\theta {\mathbf{v}}_{\perp }+sin\theta (\mathbf{u}\times {\mathbf{v}}_{\perp })$$
  由于：
  $$u{v}_{\perp }=\left[\begin{array}{c}-\mathbf{u}\cdot {\mathbf{v}}_{\perp }\\ \mathbf{u}\times {\mathbf{v}}_{\perp }\end{array}\right]$$
  因为${\mathbf{v}}_{\perp }$正交与$\mathbf{u}$，所以上述公式可以变为：

  $$u{v}_{\perp }=\left[\begin{array}{c}0\\ \mathbf{u}\times {\mathbf{v}}_{\perp }\end{array}\right]=\mathbf{u}\times {\mathbf{v}}_{\perp }$$

  所以：
  $$v_{\perp }^{{}^{\prime }}=cos\theta v_{\perp }+sin\theta (u{v}_{\perp })=(cos\theta +usin\theta )v_{\perp }$$

  令$q=cos\theta +usin\theta$，上式变为：
  $$v^{{}^{\prime }}=q{v}_{\perp }$$

  如果我们能构造一个四元数$q$，那么我们就能完成这个旋转了。
  $$q=cos\theta +usin\theta =cos\theta +i{u}_{x}sin\theta +j{u}_{y}sin\theta +k{u}_{z}sin\theta$$
  如果$\mathbf{u}$的模长为1，那么构造的q为单位四元数，它所代表的变换并不会对原向量进行缩放，是一个纯旋转。

  第三步获得$v^{{}^{\prime }}$的结果：
  $$v^{{}^{\prime }}=v_{||}^{{}^{\prime }}+v_{\perp }^{{}^{\prime }}=v_{||}+q{v}_{\perp }$$
  其中$q=\left[\begin{array}{c}cos\theta \\ usin\theta \end{array}\right]$

  第四步将化简为最终结果：
  因为：
  $$v^{{}^{\prime }}=v_{||}+q{v}_{\perp }=1v_{||}+q{v}_{\perp }=p{p}^{-1}{v}_{||}+pp{v}_{\perp }$$
  其中$q=p^2$,则$p=\left[\begin{array}{c}cos(\frac{1}{2}\theta )\\ usin(\frac{1}{2}\theta )\end{array}\right]$
  因为$u$为单位3维向量，则$p$为单位四元数，故可得：
  $$p^{-1}=p^{\ast }$$

  将上式带入之前的等式可得：
  $$v^{{}^{\prime }}=p{p}^{-1}{v}_{||}+pp{v}_{\perp }=p{p}^{\ast }{v}_{||}+pp{v}_{\perp }$$

  在化简上述公式之前，我们还需要知道两个性质：

  1. 假设$v_{||}=\left[\begin{array}{c}0\\ \mathbf{v}\end{array}\right]$是一个纯四元数，而$q=\left[\begin{array}{c}\alpha \\ \beta \mathbf{u}\end{array}\right]$，其中$u$是一个单位向量，, ∈ R．在这种条件下，如果$v_{||}$ 平行于$u$，那么$q{v}_{||}=v_{||}q$

  2. 假设$v_{\perp }=\left[\begin{array}{c}0\\ \mathbf{v}\end{array}\right]$是一个纯四元数，而$q=\left[\begin{array}{c}\alpha \\ \beta \mathbf{u}\end{array}\right]$，其中$u$是一个单位向量，, ∈ R．在这种条件下，如果$v_{\perp }$正交于$u$，那么$q{v}_{||}=v_{||}{q}^{\ast }$

  通过上述得到的引理，我们可以将$v^{{}^{\prime }}=p{p}^{\ast }{v}_{||}+pp{v}_{\perp }$化简为：
  $$v^{{}^{\prime }}=p{p}^{\ast }{v}_{||}+pp{v}_{\perp }=p{v}_{||}{p}^{\ast }+p{v}_{\perp }{p}^{\ast }=p(v_{||}+v_{\perp })p^{\ast }=pv{p}^{\ast }$$
  其中$p=\left[\begin{array}{c}cos(\frac{1}{2}\theta )\\ usin(\frac{1}{2}\theta )\end{array}\right]$,$u$为单位3维向量。

  推导完成。

  如果我们想要获得角度，只需要：
  $$\frac{\theta }{2}=arccos(a)$$
  如果要求单位向量：
  $$\mathbf{u}=\frac{\mathbf{b}}{sin(arccos(a))}$$