IMU preintegration on manifold 学习笔记（一）

IMU preintegration on manifold 学习笔记

李群流形性质

hat($\bullet ^ \wedge$) vee($\bullet ^\vee$)

$\mathbf{\omega}^{\wedge}=\begin{bmatrix}\omega\_1\\ \omega\_2\\ \omega\_3\end{bmatrix}^{\wedge}=\begin{bmatrix}0&-\omega\_3&\omega\_2\\ \omega\_3&0&-\omega\_1\\ -\omega\_2&\omega\_1&0\end{bmatrix}=\mathbf{W} \\ \mathbf{W}^{\vee}=\begin{bmatrix}0&-\omega\_3&\omega\_2\\ \omega\_3&0&-\omega\_1\\ -\omega\_2&\omega\_1& 0 \end{bmatrix}^{\vee}=\begin{bmatrix}\omega\_1\\ \omega\_2\\ \omega\_3\end{bmatrix}=\mathbf{\omega}$

hat 运算性质：

$\textbf{a}^{\wedge}\cdot\textbf{b}=-\textbf{b}^{\wedge}\cdot\textbf{a},\quad\forall\textbf{a},\textbf{b}\in R^3$

指数映射$\text{exp}\left(\bullet\right)$

$\exp\left(\vec{\phi}^{\wedge}\right)=\mathbf{I}+\dfrac{\sin\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|}\vec{\phi}^{\wedge}+\dfrac{1-\cos\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|^2}\left(\vec{\phi}^{\wedge}\right)^2$

当$\vec{\phi}$是小量时,有一阶近似：

$\text{}\exp\left(\vec{\phi}^{\wedge}\right)\approx\mathbf{I}+\vec{\phi}^{\wedge}$

对数映射$\log\left(\bullet\right)$

$\begin{aligned}\log\left(\textbf{R}\right)&=\frac{\varphi\cdot\left(\textbf{R}-\textbf{R}^T\right)}{2\sin\left(\varphi\right)}\end{aligned} \\ \varphi=\cos^{-1}\left(\dfrac{tr\big(\mathbf{R}\big)-1}{2}\right)$

得：

$\begin{aligned}\log\left(\textbf{R}\right)^\vee=\varphi\cdot\textbf{a}\end{aligned} \\ \quad\textbf{a}=\left(\dfrac{\textbf{R}-\textbf{R}^T}{2\sin\left(\varphi\right)}\right)^\vee$

其中：$\varphi$为旋转角, $\textbf{a}$为旋转轴单位矢量.当$|\vec{\phi}|<\pi$

定义符号，表示李群（$3\times 3$正交矩阵）和李代数（$3\times 3$反对称矩阵）之间的映射：

$\begin{aligned}&\text{Exp}:R^3\ni\vec{\phi}\to\exp\left(\vec{\phi}^{\wedge}\right)\in SO(3)\\ &\text{Log}:S O(3)\ni\mathbf{R}\to\log\left(\mathbf{R}\right)^{\vee}\in R^3\end{aligned}$

对于3维实向量$\vec{\phi}$和一个小量$\delta\vec{\phi}$,有下述近似性质:

$\begin{aligned}\text{Exp}\left(\vec{\phi}+\delta\vec{\phi}\right)\approx\text{Exp}\left(\vec{\phi}\right)\cdot\text{Exp}\left(\mathbf{J}\_r\left(\vec{\phi}\right)\cdot\delta\vec{\phi}\right)\\ \text{Log}\left(\text{Exp}\left(\vec{\phi}\right)\cdot\text{Exp}\left(\delta\vec{\phi}\right)\right)=\vec{\phi}+\mathbf{J}\_r^{-1}\left(\vec{\phi}\right)\cdot\delta\vec{\phi}\end{aligned}$

理解为:

$\mathbf{R}^3$中的向量$\vec{\phi}$加上一个小量$\delta\vec{\phi}$，对应到$SO(3)$中则$Exp(\vec{\phi})$是右乘一个$\mathrm{Exp}\left(\mathbf{J}\_{r}\left(\vec{\phi}\right)\cdot\delta\vec{\phi}\right)$。

$SO(3)$中$Exp(\vec{\phi})$右乘一个$Exp(\delta\vec{\phi})$，对应到$\mathbf{R}^3$中则是$\vec{\phi}$加上一项$\textbf{J}\_r^{-1}\Big(\vec{\phi}\Big)\cdot\delta\vec{\phi}$。

$\textbf{J}\_r \Big(\vec{\phi}\Big)$是$SO(3)$的右Jacobian，将切空间的“加性项”和$SO(3)$“乘性项”联系在了一起。

$\mathbf{J}\_{r}\left(\vec{\phi}\right)=\mathbf{I}-\frac{1-\cos\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|^{2}}\vec{\phi}^{\wedge}+\frac{\left\|\vec{\phi}\right\|-\sin\left(\left\|\vec{\phi}\right\|\right)}{\left\|\vec{\phi}\right\|^{3}}\left(\vec{\phi}^{\wedge}\right)^{2} \\ \mathbf{J}\_r^{-1}\left(\vec{\phi}\right)=\mathbf{I}+\dfrac{1}{2}\vec{\phi}^{\wedge}+\left(\dfrac{1}{\left\|\vec{\phi}\right\|^2}-\dfrac{1+\cos\left(\left\|\vec{\phi}\right\|\right)}{2\cdot\left\|\vec{\phi}\right\|\cdot\sin\left(\left\|\phi\right\|\right)}\right)\left(\vec{\phi}^{\wedge}\right)^2$

指数映射Adjoint：

$\begin{array}{l}\mathbf{R}\cdot\mathrm{Exp}\left(\vec{\phi}\right)\cdot\mathbf{R}^T=\mathrm{exp}\left(\mathbf{R}\vec{\phi}^{\wedge}\mathbf{R}^T\right)=\mathrm{Exp}\left(\mathbf{R}\vec{\phi}\right)\\ \Leftrightarrow\mathrm{Exp}\left(\vec{\phi}\right)\cdot\mathbf{R}=\mathbf{R}\cdot\mathrm{Exp}\left(\mathbf{R}^T\vec{\phi}\right)\end{array}$

证明：
对于任意旋转矩阵$\mathbf{R}$和三维矢量$\vec{\phi}$, 有如下恒等式：

$\left(\mathbf{R}\vec{\phi}\right)^{\wedge}=\mathbf{R}\vec{\phi}^{\wedge}\mathbf{R}^T$

由指数映射（罗德里格斯公式）：

$\begin{array}{l}\text{Exp}\left(\vec{\phi}\right)=\exp\left(\vec{\phi}\wedge\right)=\exp\left(\theta\mathbf{a}^\wedge\right)\\ =\cos\theta\cdot\mathbf{I}+\left(1-\cos\theta\right)\mathbf{a}\mathbf{a}^T+\sin\theta\cdot\mathbf{a}^\wedge\end{array}$

其中$\vec{\phi} = \theta \mathbf{a}$,为$\theta$模值,$\mathbf{a}$为单位矢量。

变形如下：

$\begin{align} &\mathbf{R}\cdot\text{Exp}\left(\vec{\phi}\right)\cdot\mathbf{R}^T\\ &=\mathbf{R}\cdot \left[\cos\theta\cdot\mathbf{I}+\left(1-\cos\theta\right)\mathbf{aa}^T+\sin\theta\cdot\mathbf{a}^\wedge\right] \cdot\mathbf{R}^T\\ &=\cos\theta\cdot\mathbf{R}\mathbf{R}^T+\left(1-\cos\theta\right)\cdot\mathbf{R}\mathbf{a}\mathbf{a}^T \mathbf{R}^T+\sin\theta\cdot\mathbf{R}\mathbf{a}^\wedge\mathbf{R}^T\\ &=\cos\theta\cdot\mathbf{I}+\left(1-\cos\theta\right)\cdot\left(\mathbf{R}\mathbf{a}\right)\cdot\left(\mathbf{R}\mathbf{a}\right)^T+\sin\theta\cdot\left(\mathbf{R}\mathbf{a}\right)^{\wedge}\\ &=\exp\left(\theta\left(\mathbf{R}\mathbf{a}\right)^{\wedge}\right)=\exp\left(\left(\mathbf{R}\vec{\phi}\right)^{\wedge}\right) \\ &=\exp\left(\mathbf{R}\vec{\phi}^{\wedge}\mathbf{R}^T\right) \\ &= \text{Exp}\left(\mathbf{R} \vec{\phi}\right) \end{align}$

IMU 运动学

陀螺仪与加速度计测量模型：

$\begin{align} &\tilde{\mathbf{\omega}}\_{wb}^b\left(t\right)=\mathbf{\omega}\_{wb}^b\left(t\right)+\mathbf{b}\_g\left(t\right)+\mathbf{\eta}\_g\left(t\right) \\ &\mathbf{f}^b\left(t\right)=\mathbf{R}\_b^{wT}\left(\mathbf{a}^w-\mathbf{g}^w\right)+\mathbf{b}\_a\left(t\right)+\mathbf{\eta}\_a\left(t\right) \end{align}$

其中$\mathbf{b}\_g,\mathbf{b}\_a$随时间缓慢变化的bias。$\mathbf{\eta}\_g,\mathbf{\eta}\_a$是白噪声。

运动模型的微分方程:

$\begin{aligned} \dot{\mathbf{R}} &= \mathbf{R} \boldsymbol{\omega}^\wedge, \\ \dot{\mathbf{p}} &= \mathbf{v} \\ \dot{\mathbf{v}} &= \mathbf{a} \end{aligned}$

进行欧拉积分得：

$\begin{align} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t) \mathrm{Exp} (\boldsymbol{\omega}(t) \Delta t) \\ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{a}(t) \Delta t \\ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t) \Delta t + \frac{1}{2} \mathbf{a}(t) \Delta t^2 \end{align}$

其中角速度和加速度可以被IMU测量到，但受到噪声与重力影响。令测量值为$\tilde{\boldsymbol{\omega}}$和$\tilde{\mathbf{a}}$：

$\begin{align} \tilde{\boldsymbol{\omega}}(t) &= \boldsymbol{\omega}(t) + \mathbf{b}\_g (t) + \boldsymbol{\eta}\_g (t) \\ \tilde{\mathbf{a}}(t) &= \mathbf{R}^\mathrm{T} (\mathbf{a}(t) - \mathbf{g}) + \mathbf{b}\_a (t) + \boldsymbol{\eta}\_a (t) \end{align}$

其中$\mathbf{b}\_g$, $\mathbf{b}\_a$为陀螺和加速度计零偏，$\boldsymbol{\eta}\_a$,$\boldsymbol{\eta}\_g$为测量的高斯噪声。把该式代入上式，可得到测量值与状态变量的关系：

$\begin{align} \mathbf{R}(t+\Delta t) &= \mathbf{R}(t) \mathrm{Exp} \left((\tilde{\boldsymbol{\omega}} - \mathbf{b}\_g(t) - \boldsymbol{\eta}\_{gd} (t))\Delta t \right) \\ \mathbf{v}(t+\Delta t) &= \mathbf{v}(t) + \mathbf{g} \Delta t + \mathbf{R}(t) (\tilde{\mathbf{a}} -\mathbf{b}\_a(t) - \boldsymbol{\eta}\_{ad}(t)) \Delta t \\ \mathbf{p}(t+\Delta t) &= \mathbf{p}(t) + \mathbf{v}(t) \Delta t + \frac{1}{2}\mathbf{g} \Delta t^2 + \frac{1}{2} \mathbf{R}(t) (\tilde{\mathbf{a}} -\mathbf{b}\_a(t) - \boldsymbol{\eta}\_{ad}(t)) \Delta t^2 \end{align}$

其中$\boldsymbol{\eta}\_{gd}$,$\boldsymbol{\eta}\_{ad}$是离散化后的随机游走噪声[6]:

$\begin{align} \mathrm{Cov}(\boldsymbol{\eta}\_{gd}(t) ) &= \frac{1}{\Delta t} \mathrm{Cov} (\boldsymbol{\eta}\_g(t)) \\ \mathrm{Cov}(\boldsymbol{\eta}\_{ad}(t) ) &= \frac{1}{\Delta t} \mathrm{Cov} (\boldsymbol{\eta}\_a(t)) \end{align}$

预积分

在优化算法中，为了避免状态量($\mathbf{R} , \mathbf{p}, \mathbf{v} $)迭代变化导致需要重新计算积分，考虑将一段时间IMU打包，称为预积分。

$\begin{align} \mathbf{R}\_j &= \mathbf{R}\_i \prod\_{k=i}^{j-1} (\mathrm{Exp} \left(\left( \tilde{\boldsymbol{\omega}}\_k - \mathbf{b}\_{g,k} - \boldsymbol{\eta}\_{gd, k} \right) \Delta t \right)\\ \mathbf{v}\_j &= \mathbf{v}\_i + \mathbf{g} \Delta t\_{ij} + \sum\_{k=i}^{j-1} \mathbf{R}\_k (\tilde{\mathbf{a}} - \mathbf{b}\_{a, k} - \boldsymbol{\eta}\_{ad, k}) \Delta t \\ \mathbf{p}\_j &= \mathbf{p}\_i + \sum\_{k=i}^{j-1} \mathbf{v}\_k \Delta t + \frac{1}{2} \mathbf{g} \Delta t\_{ij}^2 + \frac{1}{2} \sum\_{k=i}^{j-1} \mathbf{R}\_k (\tilde{\mathbf{a}}\_k - \mathbf{b}\_{a,k} - \boldsymbol{\eta}\_{ad, k}) \Delta t^2 \end{align}$

其中$\Delta t\_{ij} = \sum\_{k=i}^{j-1} \Delta t$，为累计时间。
将上式变换，定义相对变化量：

$\begin{align} \Delta \mathbf{R}\_{ij} &= \mathbf{R}\_i^\mathrm{T} \mathbf{R}\_j = \prod\_{k=i}^{j-1} \mathrm{Exp} \left( \left(\tilde{\boldsymbol{\omega}}\_k - \mathbf{b}\_{g,k} - \boldsymbol{\eta}\_{gd, k}\right) \Delta t\right) \\ \Delta \mathbf{v}\_{ij} &= \mathbf{R}\_i^\mathrm{T}(\mathbf{v}\_j - \mathbf{v}\_i -\mathbf{g} \Delta t\_{ij}) = \sum\_{k=i}^{j-1} \Delta \mathbf{R}\_{ik} (\tilde{\mathbf{a}}\_k - \mathbf{b}\_{a,k} - \boldsymbol{\eta}\_{ad, k}) \Delta t\\ \Delta \mathbf{p}\_{ij} &= \mathbf{R}\_i^\mathrm{T} \left( \mathbf{p}\_j - \mathbf{p}\_i - \mathbf{v}\_i \Delta t\_{ij} - \frac{1}{2} \mathbf{g} \Delta t\_{ij}^2 \right) \\ &= \sum\_{k=i}^{j-1} \left[\Delta \mathbf{v}\_{ik} \Delta t+\frac{1}{2} \Delta \mathbf{R}\_{ik} \left(\tilde{\mathbf{a}}\_k - \mathbf{b}\_{a,k} - \boldsymbol{\eta}\_{ad, k} \right)\Delta t^2 \right] \end{align}$

---

Reference

[1] https://zhuanlan.zhihu.com/p/388859808

[2] https://github.com/PetWorm/IMU-Preintegration-Propogation-Doc

# algorithm # math # vslam

直线拟合

Inertial-Only Optimization for Visual-Inertial Initialization 学习笔记