本文内容均默认使用右手定则，且旋转向量在局部坐标系中表达（即右乘）

• 代码实现

1. 旋转表示

1.1 旋转向量

• 用$\mathit{\varphi }=\varphi \mathit{n}$表示绕着单位轴$\mathit{n}$旋转$\varphi$弧度，用$\mathit{\omega }$表示旋转角速率

1.2 旋转矩阵

• 旋转行为： $$\begin{array}{}\text{(1)}& {\mathit{x}}^{{}^{\prime }}=\mathit{R}\mathit{x}\end{array}$$

• 约束： $$\begin{array}{}\text{(2)}& \mathit{R}{\mathit{R}}^{\mathrm{T}}={\mathit{R}}^{\mathrm{T}}\mathit{R}=\mathit{I},det(\mathit{R})=1\end{array}$$

• 微分： $$\begin{array}{}\text{(3)}& \dot{\mathit{R}}=\mathit{R}{\mathit{\omega }}^{\wedge }\end{array}$$

1.3 四元数

• 表示： $$\begin{array}{}\text{(4)}& \mathit{q}=[s,\mathit{v}{]}^{\mathrm{T}},s=q_w\in \mathbb{R},\mathit{v}={[q_x,q_y,q_z]}^{\mathrm{T}}\in {\mathbb{R}}^3\end{array}$$

  只有单位四元数才能表示旋转

• 四元数乘法： $$\begin{array}{}\text{(5)}& {\mathbf{q}}_a\otimes {\mathbf{q}}_b=\left[\begin{array}{c}{s}_a{s}_b-{\mathbf{v}}_a^{\mathrm{\top }}{\mathbf{v}}_b\\ s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\end{array}\right]\end{array}$$

  !!!四元数乘法不满足乘法结合律

• 旋转行为： $$\begin{array}{}\text{(6)}& {\mathit{x}}^{{}^{\prime }}=\mathit{q}\otimes \mathit{x}\otimes {\mathit{q}}^{\ast }\end{array}$$

• 约束： $$\begin{array}{}\text{(7)}& \mathbf{q}\otimes {\mathbf{q}}^{\ast }={\mathbf{q}}^{\ast }\otimes \mathbf{q}=1\end{array}$$

• 微分： $$\begin{array}{}\text{(8)}& \dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \mathit{\omega }\end{array}$$

2. 旋转转换

2.1 旋转向量与旋转矩阵

2.1.1 从旋转向量转换到旋转矩阵的转换

$$\begin{array}{}\text{(9)}& \begin{array}{rl}\mathit{R}& =\exp \left({\mathit{\varphi }}^{\wedge }\right)=\sum _{n=0}^{\mathrm{\infty }}\frac{1}{n!}{\left({\mathit{\varphi }}^{\wedge }\right)}^n\approx \mathit{I}+{\mathit{\varphi }}^{\wedge }\\ & =\exp \left(\varphi {\mathit{n}}^{\wedge }\right)=\cos \varphi \mathit{I}+(1-\cos \varphi )\mathit{n}{\mathit{n}}^{\mathrm{T}}+\sin \varphi {\mathit{n}}^{\wedge }\end{array}\end{array}$$

式(9)的第二行即罗德里格斯公式

2.1.2 从旋转矩阵转换到旋转向量的转换

$$\begin{array}{}\text{(10)}& \begin{array}{rl}\varphi & =\arccos \frac{\mathrm{tr}(\mathit{R})-1}{2}\\ \mathit{n}& =\frac{{(\mathit{R}-{\mathit{R}}^{\mathrm{\top }})}^{\vee }}{2\sin \varphi }\end{array}\end{array}$$

2.2 旋转向量与四元数

2.2.1 从旋转向量到四元数的转换

将$\mathit{\varphi }$转换为纯四元数（pure quaternion），则有 $$\begin{array}{}\text{(11)}& \mathit{q}=exp(\frac{\mathit{\varphi }}{2})=\left[\begin{array}{c}\cos \frac{\varphi }{2}\\ \mathit{n}\sin \frac{\varphi }{2}\end{array}\right]\approx \left[\begin{array}{c}1\\ \frac{1}{2}\mathit{\varphi }\end{array}\right]\end{array}$$

上式中存在$\mathit{\varphi }$到纯四元数（pure quaternion）的隐含转换

2.2.2 从四元数到旋转向量的转换

$$\begin{array}{}\text{(12)}& \begin{array}{rl}\varphi & =2\arctan (||\mathit{v}||,s)\\ \mathit{n}& =\frac{\mathit{v}}{||\mathit{v}||}\end{array}\end{array}$$

2.3 旋转矩阵与四元数

2.3.1 从旋转矩阵到四元数的转换

$$\begin{array}{}\text{(13)}& \{\begin{array}{r}{q}_w=sign(q_w)\frac{1}{2}\sqrt{1+r_{11}+r_{22}+r_{33}}\\ q_x=sign(q_x)\frac{1}{2}\sqrt{1+r_{11}-r_{22}-r_{33}}\\ q_y=sign(q_y)\frac{1}{2}\sqrt{1-r_{11}+r_{22}-r_{33}}\\ q_z=sign(q_z)\frac{1}{2}\sqrt{1-r_{11}-r_{22}+r_{33}}\end{array}\end{array}$$

2.3.2 从四元数到旋转矩阵的转换

$$\begin{array}{}\text{(14)}& \mathit{R}=(s^2-{\mathit{v}}^{\mathrm{T}}\mathit{v})\mathit{I}+2\mathit{v}{\mathit{v}}^{\mathrm{T}}+2s{\mathit{v}}^{\wedge }\end{array}$$

3. 旋转更新

• 在优化姿态时，通常计算一个旋转向量增量$\delta \mathit{\varphi }$，再用它更新当前的估计值$\mathbf{R}\leftarrow \mathbf{R}\exp \left(\mathit{\delta }{\mathit{\varphi }}^{\wedge }\right)$或$\mathbf{q}\leftarrow \mathbf{q}\otimes exp(\frac{\mathit{\delta }\mathit{\varphi }}{2})$

参考文献

1. 《视觉SLAM十四讲》
2. Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.
3. 《机器人学中的状态估计》
4. 《多旋翼飞行器设计与控制》
5. 深蓝学院《从零手写VIO》课程

奇异值分解求解线性方程组

奇异值分解（SVD）：

设$\mathit{A}\in {\mathbb{C}}_r^{m\times n}$，则存在正交矩阵$\mathit{U}\in {\mathbb{C}}^{m\times m}$与正交矩阵$\mathit{V}\in {\mathbb{C}}^{n\times n}$使得$\mathit{A}=\mathit{U}\left[\begin{array}{cc}{\mathbf{\Sigma }}_r& \mathbf{0}\\ \mathbf{0}& \mathbf{0}\end{array}\right]{\mathit{V}}^T=\mathit{U}\mathit{D}{\mathit{V}}^T$，其中${\mathbf{\Sigma }}_{\mathit{r}}=diag({\sigma }_1,...,{\sigma }_r)$是矩阵$\mathit{A}$的正奇异值

• 通过奇异值分解将原问题转换为最小二乘问题，可以求解超定线性方程组

1 求解齐次线性方程组（$\mathit{A}\mathit{x}=\mathbf{0}$）

$$ 令$\mathit{y}={\mathit{V}}^T\mathit{x}$，则$(1)$式变为$\underset{\mathit{y}}{min}||\mathit{D}\mathit{y}|{|}^2=\underset{\mathit{y}}{min}({\sigma }_1^2{y}_1^2+{\sigma }_2^2{y}_2^2+...+{\sigma }_r^2{y}_r^2)$，其中$r<=n$

因为${\sigma }_1>{\sigma }_2>...>{\sigma }_r>0$，所有不管$r$是否等于$n$，在$||\mathit{x}||=1$的前提下的最优解均在$\mathit{y}=[0,0,...,1{]}^T$时取得（$r=n$时存在最小二乘解，$r<n$时存在数值解），此时$\mathit{x}=\mathit{V}\mathit{y}$ ，即$\mathit{V}$的最后一列

2 求解非齐次线性方程组（$\mathit{A}\mathit{x}=\mathit{b}$）

问题等价于$\underset{\mathit{x}}{min}||\mathit{A}\mathit{x}-\mathit{b}|{|}^2$，则 $$\begin{array}{}\text{(2)}& \begin{array}{rl}& \underset{\mathit{x}}{min}||\mathit{A}\mathit{x}-\mathit{b}|{|}^2\\ =& \underset{\mathit{x}}{min}||\mathit{U}\mathit{D}{\mathit{V}}^T\mathit{x}-\mathit{b}|{|}^2\\ =& \underset{\mathit{x}}{min}||\mathit{D}{\mathit{V}}^T\mathit{x}-{\mathit{U}}^T\mathit{b}|{|}^2\end{array}\end{array}$$ 令$\mathit{y}={\mathit{V}}^T\mathit{x}$，$\mathit{c}={\mathit{U}}^T\mathit{b}$，则$(2)$式变为$\underset{\mathit{y}}{min}||\mathit{D}\mathit{y}-\mathit{c}|{|}^2=\underset{\mathit{y}}{min}(\sum _{i=1}^r({\sigma }_i{y}_i-c_i{)}^2+\sum _{i=r+1}^m{c}_i^2)$

当$y_i=\frac{c_i}{{\sigma }_i}(i=1,...,r)$而$y_i(i=r+1,...,n)$为任意值时求得最优解

参考文献

1. 王磊.矩阵基本理论与应用[M]
2. https://blog.csdn.net/Ansel_Lee/article/details/107643789
3. https://blog.csdn.net/MyArrow/article/details/53780972

本质矩阵$\mathit{E}={\mathit{t}}^{\wedge }\mathit{R}$的奇异值必定是$[\sigma ,\sigma ,0{]}^T$的形式，这称为本质矩阵的内在性质

证明过程

$\because \mathit{E}=\mathit{U}\mathbf{\Sigma }{\mathit{V}}^T$

$ ^T = ^T ( ^T)T = ^TT =^T $

又$\because \mathit{E}{\mathit{E}}^T={\mathit{t}}^{\wedge }\mathit{R}({\mathit{t}}^{\wedge }\mathit{R}{)}^T={\mathit{t}}^{\wedge }\mathit{R}{\mathit{R}}^T{\mathit{t}}^{\wedge T}={\mathit{t}}^{\wedge }{\mathit{t}}^{\wedge T}=\left[\begin{array}{ccc}0& -t_z& t_y\\ t_z& 0& -t_x\\ -t_y& t_x& 0\end{array}\right]\left[\begin{array}{ccc}0& t_z& -t_y\\ -t_z& 0& t_x\\ t_y& -t_x& 0\end{array}\right]$

考虑到$\mathit{E}$的尺度等价性（$\mathit{E}$只有5个自由度），将$\mathit{t}=[t_x{t}_y{t}_z{]}^T$归一化为单位矢量，即$t_x^2+t_y^2+t_z^2=1$

那么可由$|\lambda \mathit{I}-\mathit{E}{\mathit{E}}^T|=0$求取$\mathit{E}{\mathit{E}}^T$的特征值

行列式展开得$\lambda (\lambda -1{)}^2=0$，即$\mathit{M}=diag({\lambda }_1,{\lambda }_2,{\lambda }_3)=diag(1,1,0)$

又$\because \mathit{M}=\mathbf{\Sigma }{\mathbf{\Sigma }}^T$

$\therefore \mathbf{\Sigma }=diag(1,1,0)$

得证

• PS:《计算机视觉中的多视图几何》第九章也给出了证明，但需要更强的数学基础

参考文献

1. https://blog.csdn.net/zengxyuyu/article/details/106676509
2. 《视觉SLAM十四讲》第二版
3. 《计算机视觉中的多视图几何》第二版

Abstract

• 作者提出了一种半直接单目视觉里程计算法，该算法精确、鲁棒，并且比当前最先进的方法更快
  • 该算法无需代价昂贵的特征提取和鲁棒匹配技术
  • 该算法直接对像素强度进行操作，在高帧率下可以达到亚像素精度
  • 使用显式建模异常测量的概率建图方法来估计3D点，从而得到更少的异常点和更可靠的点
  • 精确的高帧率运动估计在纹理较少、重复和高频的场景中具有更高的鲁棒性
• 该算法应用于GPS拒止环境下的微型飞行器状态估计，在机载嵌入式计算机上以55帧/秒的速度运行，在消费级笔记本电脑上以超过300帧/秒的速度运行
• 该算法称为SVO (Semi-direct Visual Odometry)，并将软件实现开源

Welcome to Hexo! This is your very first post. Check documentation for more info. If you get any problems when using Hexo, you can find the answer in troubleshooting or you can ask me on GitHub.

Quick Start

Create a new post

1

$ hexo new "My New Post"

More info: Writing

Run server

1

$ hexo server

More info: Server

Generate static files

1

$ hexo generate

More info: Generating

Deploy to remote sites

1

$ hexo deploy

More info: Deployment