【VSLAM面试】必备基础知识1-旋转，李群李代数，Ax=0，Ax=b解法

【VSLAM面试】必备基础知识1-旋转，李群李代数，Ax=0，Ax=b解法

笔者秋招告一段落了，总结一下常用的VSLAM，VIO面试基础知识，做个记录，也回馈给大家，共同进步。

旋转的四种表示方式

方法	自由度	简单理解	优点	缺点
旋转矩阵	9	$p^{{}^{\prime }}=Rp$	较为直观	用9个量描述旋转的3自由度，具有冗余性 自身具有约束：正交矩阵且行列式为1
旋转向量 （轴角）	3	用一个旋转轴和一个绕轴的旋转角来描述旋转	几何上较为直观	不实用（很难找到旋转轴的位置）
欧拉角	3	将一个旋转分解成3次绕不同轴的旋转	参数少，几何上较为直观	存在奇点（万向锁问题：某位置自由度缺失，当俯仰角伪±90°时，第一次旋转和第三次旋转将使用同一个轴，使得系统丢失了一个自由度），不适与插值和迭代
四元数	4	复数的推广 $p^{{}^{\prime }}=qp{q}^{-1}$	紧凑且没有歧义性，速度更快、提供平滑插值（球面线性插值）、有效避免万向锁问题、存储空间较小等	几何上较难理解，运算比较复杂

• 旋转矩阵与旋转向量

  • 旋转向量$n\theta$→旋转矩阵$R$：罗德里格斯公式

    $R=cos\theta I+(1-cos\theta )n{n}^T+sin\theta \hat{n}$，$n$为旋转轴单位向量，$\theta$为旋转角度，$\hat{n}$为向量到反对称矩阵的运算符。

  • 旋转矩阵→旋转向量

    由$tr(R)=cos\theta \cdot tr(I)+(1-cos\theta )\cdot tr(n{n}^T)+sin\theta \cdot tr(\hat{n})=3cos\theta +(1-cos\theta )=1+2cos\theta$

    可得转角$\theta =arccos\frac{tr(R)-1}{2}$

    对于转轴$n$，旋转轴上的向量在旋转后不发生改变：$Rn=n$，因此，转轴$n$是矩阵$R$特征值1对应的特征向量。

• 欧拉角与旋转矩阵

  • 欧拉角→旋转矩阵（欧拉角外旋$ZYX$，每个旋转绕的是惯性系的轴）

    $R=R_Z(\alpha )R_Y(\beta )R_X(\gamma )=\left[\begin{array}{ccc}cos\alpha & -sin\alpha & 0\\ sin\alpha & cos\alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}cos\beta & 0& sin\beta \\ 0& 1& 0\\ -sin\beta & 0& cos\beta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\gamma & sin\gamma \\ 0& sin\gamma & cos\gamma \end{array}\right]=\left[\begin{array}{ccc}c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma -s\alpha s\gamma \\ s\alpha c\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma \\ -s\beta & c\beta s\gamma & c\beta c\gamma \end{array}\right]$

  • 旋转矩阵→欧拉角（旋转矩阵的解析表达式+反三角函数去凑欧拉角）从$ZXY$顺规旋转矩阵提取欧拉角的公式

    $y$轴$yaw$角：$\alpha =atan2(sin\alpha cos\beta ,cos\alpha cos\beta )=atan2(m_{13},m_{33})$

    $x$轴$pitch$角：$\alpha =arcsin(sin\beta )=arcsin(-m_{23})$

    $z$轴$roll$角：$\alpha =atan2(cos\beta sin\gamma ,cos\beta cos\gamma )=atan2(m_{21},m_{22})$

• 四元数与旋转向量

  • 已知旋转向量，求四元数：绕某条单位轴旋转一定角度，绕单位轴$n$旋转$\theta$角的四元数

    旋转向量$n\theta$，绕单位向量$n=[n_x,n_y,n_z{]}^T$，旋转角度$\theta$，四元数$q(q_0,q)=(cos\frac{\theta }{2},nsin\frac{\theta }{2})=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{]}^T$

  • 已知四元数，求旋转向量

    旋转角度$\theta =2arccos{q}_0$，单位旋转向量$n=[n_x,n_y,n_z{]}^T=[q_1,q_2,q_3{]}^T/sin\frac{\theta }{2}$

• 四元数与旋转矩阵

  • 已知四元数$q=[q_0,q_1,q_2,q_3]$，求旋转矩阵$R$：把四元数的旋转操作写成矩阵的形式

    $R=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$

  • 已知旋转矩阵$R$，求四元数$q$：用参数化过的矩阵表达式凑四元数

    $q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}$

    $tr(R)$是旋转矩阵$R$的迹，也就是矩阵对角元素之和

李群李代数

SLAM问题经常构建与位姿相关的优化函数，需要计算优化函数对位姿的导数。然而SO(3)和SE(3)是群，没有良好的加法定义，如果当成一个普通矩阵来处理优化，那么该优化问题是一个带约束的优化问题。具体来说用旋转矩阵和变换矩阵描述位姿，会引入旋转矩阵自身的约束，即正交且行列式为1($R^{T}R=I$，$det(R)=1$)，他们作为优化变量时，会使得优化变的困难，通过李群李代数的转换关系，从李代数角度求解优化函数，由于李代数由向量组成，具有良好的加法运算，可以把位姿估计变成无约束的优化问题。

• SO(3)上的李代数求导
  • 左扰动模型
  • 右扰动模型
    
• SE(3)上的李代数求导

  • 左扰动模型
    

  • 右扰动模型
    

$Ax=0$的解法？

• 求解线性方程组——如果矩阵$A$是方阵，可以使用求解线性方程组的方法来求解$x$
• 矩阵分解——使用SVD

$Ax=b$的几种解法？

• 闭式解（解析解）

  求解析解，即导函数为0，$dF/dx=0$

  $Ax=b\Rightarrow A^{T}Ax=A^{T}b\Rightarrow x=(A^{T}A{)}^{-1}{A}^{T}b$

• 优化解（优化）

  采用迭代的方法，从一个初始值出发，不断寻找下降增量$△x$更新当前优化变量，使目标函数下降。

  当$A^{T}A$为奇异矩阵时（不可逆），最小二乘法的解析解无法计算，导致$A^{T}A$不可逆的原因可能是$A$中的列向量线性相关，即数据中存在特征冗余，某些特征可以根据其它特征的线性组合来得到。

  奇异矩阵：对于一个方阵，若该方阵的行列式等于0，则称该方阵为奇异矩阵，若不等于0，则称为非奇异矩阵。由此：不可逆阵$\iff$奇异矩阵

  事实上，除了不可逆外，当$A^{T}A$为病态矩阵(ill-conditioned matrix)时，最小二乘法也会失效，此时虽然也能按照解析解进行计算，但由于解的方差过大，不稳定，所以结果精度很差。

  病态矩阵：即求解方程组时对数据的微小扰动比较敏感的矩阵。比如在求解方程组$Ax=b$时，如果对系数矩阵$A$或右端项$b$进行一个微小的扰动，算出的解和原方程组的解差别很大，这样的矩阵$A$被称为病态矩阵。

  用条件数来判断$A$矩阵是否为病态矩阵，对于这种一个微小扰动对数值求解造成很大干扰的病态矩阵直接跳过

  条件数定义为矩阵的范数乘以其逆矩阵的范数，即$cond(A)=\Vert A\Vert \cdot \Vert A^{-1}\Vert$，条件数越大，矩阵越病态，越接近不可逆，从而也越难得到精确解。