从零开始学习VIO笔记 --- 第一讲：基础知识（四元数，李代数）

一. 向量的内积与外积

主要针对三维向量，两个三维向量必定共面；
内积 ：
$$a\cdot b=a_1{b}_1+a_2{b}_2+a_3{b}_3=\sum _{i=1}^3{a}_i{b}_i=|a||b|\cos ⟨a,b⟩$$
为标量
外积/叉乘 ：
$$a\times b=\left[\begin{array}{c}\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\end{array}\right]b=a^{\wedge }b$$
外积结果为矢量，此矢量与向量 $a$ 和向量 $b$ 都垂直；它的大小为$|a||b|\sin ⟨a,b⟩$，大小等于向量 $a$，$b$ 组成的平行四边形面积大小；

$a^{\wedge }$ 为向量 $a$ 的反对称矩阵 $A$；（任意向量都对应着唯一的一个反对称矩阵）

$a^{\wedge }=A$
$A^{\vee }=a$

二. 旋转与平移

2.1 旋转表示 — 旋转矩阵R

有坐标系 $a$，基为 $a=\left[\begin{array}{ccc}{a}_1,& a_2,& a_3\end{array}\right]$ （注：为3 $\times$ 3的单位正交矩阵） ，在坐标系 $a$ 下，有一个空间点 $x$，坐标为 $\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]$ ，坐标系 $a$ 经过旋转后为坐标系 $b$，基为 $b=\left[\begin{array}{ccc}{b}_1,& b_2,& b_3\end{array}\right]$，$x$点在坐标系 $b$ 下的坐标为 $\left[\begin{array}{c}{x}_1^{\prime }\\ x_2^{\prime }\\ x_3^{\prime }\end{array}\right]$ 。对于 $x$点，它并未发生位置变换，向量关系没有改变，所以有下式成立:

$$\left[\begin{array}{ccc}{a}_1,& a_2,& a_3\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]=\left[\begin{array}{ccc}{b}_1,& b_2,& b_3\end{array}\right]\left[\begin{array}{c}{x}_1^{\prime }\\ x_2^{\prime }\\ x_3^{\prime }\end{array}\right]$$
点 $x$在坐标系 $b$ 下便可以通过如下表示:

$$\begin{array}{l}\left[\begin{array}{c}{x}_1^{\prime }\\ x_2^{\prime }\\ x_3^{\prime }\end{array}\right]={(\left[\begin{array}{ccc}{b}_1,& b_2,& b_3\end{array}\right])}^{-1}\left[\begin{array}{ccc}{a}_1,& a_2,& a_3\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]={(\left[\begin{array}{clc}{b}_1,& b_2,& b_3\end{array}\right])}^T\left[\begin{array}{ccc}{a}_1,& a_2,& a_3\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\\ =\left(\left[\begin{array}{c}{b}_1\\ b_2\\ b_3\end{array}\right]\left[\begin{array}{ccc}{a}_1,& a_2,& a_3\end{array}\right]\right)\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]=R_{ba}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\end{array}$$
即:
$$\begin{array}{l}\left[\begin{array}{c}{x}_1^{\prime }\\ x_2^{\prime }\\ x_3^{\prime }\end{array}\right]=R_{ba}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\end{array}$$
（注：直角坐标系的基是3 $\times$ 3的正交矩阵，行列式为1，并相互正交，为单位正交基；
基与基相乘依旧为正交基，有 $\begin{array}{l}{a}^{T}a=I{a}^{-1}=a^T\end{array}$ )

旋转矩阵即为 $R_{ba}$ ，称坐标系 $a$ 到 $b$ 的旋转矩阵

2.2 平移向量

向量 $\chi$ 经过旋转和平移之后为 ${\chi }^{\prime }$ ,其中旋转矩阵为 $R$ ，平移向量为 $t=\left[\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right]$
$${\chi }^{\prime }=R\chi +t$$

2.3 变换矩阵T与齐次坐标

为了方便进行坐标系变换中的乘法运算，我们引入变换矩阵 $T=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]$；为了对其维数，引入齐次坐标$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ 1\end{array}\right]$ ，则有：
$$\left[\begin{array}{c}{X}^{{}^{\prime }}\\ 1\end{array}\right]=\left[\begin{array}{cc}R& t\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}X\\ 1\end{array}\right]=T\left[\begin{array}{c}X\\ 1\end{array}\right]$$

2.4 旋转表示 — 旋转向量与欧拉角

旋转向量
任意旋转可以通过一个旋转轴和旋转角表示；即可以通过旋转向量（或角轴，为三维向量）表示，旋转向量的方向与旋转轴方向一至，模长为旋转角度；
假设单位向量$n=\left[\begin{array}{c}{n}_1\\ n_2\\ n_3\end{array}\right]$ 与 旋转轴方向一至，旋转角度为 $\theta$ ,则旋转向量为 $\theta n$

旋转向量转换到旋转矩阵R ：(罗德里格斯公式)
$$R=\cos \left(\theta I\right)+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$$
欧拉角

能直观的表示旋转，但是会出现万向锁问题

1. 绕 Z 轴转，偏航角 yaw
2. 绕 Y 轴转，俯仰角 pitch
3. 绕 X 轴转，翻滚角 roll
   

2.5 旋转表示 — 四元数

2.5.1 四元数

四元数用 $q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]=\left[\begin{array}{c}s\\ v\end{array}\right]$ 四维向量表示 ；$q_0$ , $s$ 为实部，${\left[\begin{array}{ccc}{q}_1,& q_2,& q_3\end{array}\right]}^T$ , $v$ 为虚部；

旋转向量 变换到 四元数q：
$$q={[\cos \frac{\theta }{2},n\sin \frac{\theta }{2}]}^T$$
四元数 $q$ 到旋转矩阵 $R$ 的变换 :
$$R=v{v}^T+s^{2}I+2s{v}^{\wedge }+(v^{\wedge }{)}^2$$

2.5.2 四元数对时间的导数

设初始旋转为 $q=[s,v]$，然后，发生了角轴为 $\omega ,\theta$ 的旋转（右乘，对应四元数记作 $△q$），那么 $q$ 相对该旋转的导数为:

因此，若角速度为 $\omega$，那么旋转的时间导数即为：

除了利用四元数求导，亦可利用李代数进行旋转求导
使用旋转矩阵 $R$ 时，角速度为 $\omega$，那么 $R$ 相对于时间的导数可写作

在优化带有旋转的函数时，通常计算一个增量 $\varphi \in so(3)$，然后用它更新当前估计值：(由于这个增量并没有指定方向，所以左乘右乘都可以)

表示旋转可以用 矩阵$R$表示，也可以用 四元数表示，是一样的形式进行更新。

2.6 李群与李代数

2.6.1 李群

群：只有一个合法的运算的集合

旋转矩阵 $R$ 构成特殊正交群 $SO(3)$
S O ( 3 ) = { R ∈ R 3 × 3    ∣    R R T = I    , d e t ( R )    =    1 } SO(3)=\{R\in\mathbb{R}^{3\times3}\;\vert\;RR^T=I\;,det(R)\;=\;1\} SO(3)={R∈R3×3∣RRT=I,det(R)=1}

变换矩阵 $T$ 构成特殊欧式群 $SE(3)$
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4    ∣    R ∈ S O ( 3 )    , t ∈ R 3 } SE(3)=\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \mathbb{R}^{4\times4}\;\vert\;R\in SO(3)\;,t\in \mathbb{R}^3\} SE(3)={T=[R0T​t1​]∈R4×4∣R∈SO(3),t∈R3}

2.6.1 李代数的引出

相机运动过程时，旋转矩阵可以用 $R(t)$，$R$ 关于时间 $t$ 的函数表示
对于旋转矩阵
$$R{R}^T=I$$
于是有
$$R(t)R(t{)}^T=I$$ 等式两边对时间求,得
$$\stackrel{.}{R}(t)R{(t)}^T+R(t)\stackrel{.}{R}{(t)}^T=0$$$$\begin{gathered} \Rightarrow \stackrel{.}{R}(t)R{(t)}^T=-R(t)\stackrel{.}{R}{(t)}^T \\ \Rightarrow \stackrel{.}{R}(t)R{(t)}^T=-{\left(\stackrel{.}{R}(t)R{(t)}^T\right)}^T \end{gathered}$$ 说明 $\stackrel{.}{R}(t)R{(t)}^T$ 为反对称矩阵，存在一个三维向量 $\varphi (t)$ 与之对应，有
$$\stackrel{.}{R}(t)R{(t)}^T=\varphi (t{)}^{\wedge }$$ 等式两边右乘 $R(t)$，得：
$$\stackrel{.}{R}(t)=\varphi (t{)}^{\wedge }R(t)$$

总结：
1. 说明对旋转矩阵求导，等价于在旋转矩阵左乘一个反对称矩阵
如何求得这个反对称矩阵呢 ？
对于$t_0$时刻， $R(0)=I$ ,把$R(t)$在$t=0$附近进行一阶泰勒展开：
$$\begin{gathered} R(t)\approx R(t_0)+\stackrel{.}{R}(t_0)(t-t_0)=R(t_0)+{\varphi (t_0)}^{\wedge }R(t_0)(t-t_0) \\ =I+{\varphi (t_0)}^{\wedge }t \end{gathered}$$
此为一个关于 $R$ 的微分方程，且 $R(0)=I$，便可解得：
$$R(t)=exp({\varphi }_0^{\wedge }t)$$

2. 说明在 $t=0$的附近，旋转矩阵可以由$exp({\varphi }_0^{\wedge }t)$，解出。

给定某个时刻的 $R$,我们可以求出一个$\varphi$ ,描述了$R$ 在局部的导数关系; $\varphi$ 是$SO(3)$对应的李代数 $so(3)$;
$$R=exp({\varphi }^{\wedge })$$

2.6.2 李代数 $so(3)$ 与$se(3)$

$\to$ 给定一个$\varphi$ 可以通过指数映射$exp({\varphi }^{\wedge })$，求出$R$
$\to$ 给定一个$R$ 可以通过对数映射，求出$\varphi$

$R$的李代数为 $so(3)$，变换矩阵 $T$ 的李代数为 $se(3)$

$so(3)$中的$\varphi$ 变换到$SO(3)$中的 $R$:
$$R=exp({\varphi }^{\wedge })$$
$SO(3)$中的 $R$ 变换到 $so(3)$中的$\varphi$:
$$\varphi ={\ln \left(R\right)}^{\vee }$$
总结：
$so(3)$ 这个李代数集合中的元素为 三维向量 $\varphi$ 或者是三维反对称矩阵${\varphi }^{\wedge }$

$se(3)$ 这个李代数集合中的元素为 六维向量 $\xi =\left[\begin{array}{c}\rho \\ \varphi \end{array}\right]$ ,其中$\rho$ 表示平移,$\varphi$ 则是上面的旋转三维向量；但是 $\rho$ 并不直接等于变换矩阵中的 $t$;对应的四维 反对称矩阵 ${\xi }^{\wedge }=\left[\begin{array}{cc}{\varphi }^{\wedge }& \rho \\ 0^T& 0\end{array}\right]$

2.6.3 李代数求导

使用李代数表示姿态，根据李代数加法运算对李代数求导
旋转之后的位姿对旋转矩阵求导：$\frac{\partial (Rp)}{\partial R}$ ，由于$SO(3)$没有加法运算，转而使用李代数加法运算去求导：
$$\frac{\partial (exp({\varphi }^{\wedge })p)}{\partial \varphi }=\underset{△\varphi \to 0}{\lim }\frac{exp({(\varphi +△\varphi )}^{\wedge })p-exp({\varphi }^{\wedge })p}{△\varphi }=...={(-Rp)}^{\wedge }{J}_l$$

2.6.4 李群左右扰动

左扰动求导：
对 $R$ 进行一次左扰动$△R$，左乘在$R$左侧,$△R$对应的李代数设为$\phi$ , $Rp$对$\phi$ 求导：
$$\begin{gathered} \frac{\partial (Rp)}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{△RRp-Rp}{\phi }=\underset{\phi \to 0}{\lim }\frac{exp({\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{(I+{\phi }^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\phi }=\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }exp({\varphi }^{\wedge })p}{\phi } \\ =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }=\underset{\phi \to 0}{\lim }\frac{-{(Rp)}^{\wedge }\phi }{\phi }=-{(Rp)}^{\wedge } \end{gathered}$$
此式比上面用李代数求导少了一个雅克比计算，所以扰动模型更实用。

旋转点右扰动求导：

三. 旋转连乘雅克比

四. 作业

4.1 题目一 — VIO 文献阅读

参考文献： “A review of visual inertial odometry from filtering and optimisation perspectives”. Advanced Robotics 29.20 (2015)

4.1.1 视觉与 IMU 进行融合之后有何优势？

4.1.2 有哪些常见的视觉 +IMU 融合方案？有没有工业界应用的例子？

4.1.3 学术界，VIO 研究有哪些新进展？有没有将（深度）学习方法用到 VIO 中的例子？

4.2 题目二 — 四元数和李代数更新

4.1.1 题目：

4.1.2 解答

需要的库:

sophus ：李群SO(3),SE(3) --- Sophus::SO3d ;   // d表示 double
eigen3 ：旋转矩阵R --- Eigen : :Matrix3d; 微小增量ω ---- Vector3d  ; 李代数(三维向量) ---- Vector3d ；
					 四元数--- Quaterniond

引入头文件

#include <iostream>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>

旋转向量 $\theta n$ （角度+单位三维向量） 转为旋转矩阵 $R$

// Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)) 旋转向量 
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();

旋转矩阵 $R$ 构造四元数 $q$

Quaterniond q(R); 

旋转矩阵 $R$ ，四元数 $q$ 构造 $SO(3)$ ; Sophus::SO3d 是一个类 ； $SO(3)$本质是矩阵 $R$；
类方法 SO3_R.matrix() 返回其旋转矩阵形式

 Sophus::SO3d SO3_R(R); // 从旋转矩阵构造Sophus::SO(3)
 Sophus::SO3d SO3_q(q); // 也可以使用四元素构造Sophus::SO(3)
std::cout << "SO3 :  "  << SO3_R.matrix() << std::endl; 

1.使用旋转矩阵更新 $R\leftarrow Rexp({\omega }^{\wedge })$ , $\omega$ 微小增量李代数，三维向量

Vector3d update_so3(0.01, 0.02, 0.03);                            //更新量 
Sophus::SO3d SO3_updated = SO3_R * Sophus::SO3d::exp(update_so3); //右乘更新

SO(3) 用矩阵R显示

std::cout << "SO3  R= \n"<< SO3_updated.matrix() <<std::endl;

2.使用四元数更新 $q\leftarrow q\otimes {\left[\begin{array}{cc}1,& \frac{1}{2}\omega \end{array}\right]}^T$

Eigen::Quaterniond update_q(1, update_so3(0) / 2, update_so3(1) / 2, update_so3(2) / 2);
update_q.normalize();   //四元数归一化处理
Eigen::Quaterniond q_updated = q * update_q;

四元数 转为 矩阵R显示

Eigen : : Matrix3d  q2R =  q_updated.toRotationMatrix() 
std::cout << "q2R = \n"<< q2R <<std::endl;

4.3 题目三 — 推导其他导数

4.3.1 题目

4.3.2 解答