VSLAM：：[手写VIO]第一讲_预备知识

1. 四元数的基本运算

• 主要运算
  
• 四元数乘法
  

乘法性质

1. 满足结合律
2. 不满足交换律
3. 乘积的模等于模的乘积
4. 乘积的逆等于各个四元数的逆以相反的顺序相乘

• 其他运算
  
  

*四元数部分参考：旋转矩阵、欧拉角、四元数理论及其转换关系

---

2. 四元数与旋转向量

简单来说，四元数的思想就是把方向余弦矩阵的三次旋转表示为只绕一个旋转轴旋转一次完成，因此可以用4个数来表示这个过程，其中包括旋转轴向量的长度(θ)和旋转轴单位向量(x,y,z)

2.1. 四元数的表示

$\begin{aligned} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} = \begin{bmatrix} \cos \frac{\theta}{2} \\ x*\sin \frac{\theta}{2} \\ y*\sin \frac{\theta}{2} \\ z*\sin \frac{\theta}{2} \\ \end{bmatrix} \end{aligned}$⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​=⎣⎢⎢⎡​cos2θ​x∗sin2θ​y∗sin2θ​z∗sin2θ​​⎦⎥⎥⎤​​

---

3. 四元数对时间的导数

3.1. 关于两个极限的近似(上面求极限用到的)

---

4. 旋转矩阵(李群SO3)对角速度$\omega$ω的求导

5. so(3) 导数

5.1. 左乘模型

5.2. 右乘模型

6. 旋转连乘的雅克比

6.1. 对R2求导

6.2. 对R1求导

6.3. 伴随性质的证明

6.3.1. 即证明下式

$\begin{aligned} R~exp(\phi \hat{~} )~R^T=exp((R\phi)\hat{~}) \end{aligned}$R exp(ϕ ^) RT=exp((Rϕ) ^)​

6.3.2. 首先证明

$Rp\hat{~}R^T=(Rp)\hat{~}$Rp ^RT=(Rp) ^
//或者:(上下是等价的)
$\begin{aligned} R[a]_\times=[Ra]_\times R \end{aligned}$R[a]×​=[Ra]×​R​

6.3.2.1. 已知

若
$\begin{aligned} a=b\times c \end{aligned}$a=b×c​
则有
$\begin{aligned} &R\in SO3 \\ &Ra=(Rb) \times (Rc) \end{aligned}$​R∈SO3Ra=(Rb)×(Rc)​

6.3.2.2. 可证

$\begin{aligned} (Rp)\hat{~}&=(Rp)\hat{~}I=(Rp)_\times RR^{T}I \\ &=(Rp)_\times (R R^{T}I) \\ &=R(p_\times R^{T}I) \\ &=Rp\hat{~}R^T \end{aligned}$(Rp) ^​=(Rp) ^I=(Rp)×​RRTI=(Rp)×​(RRTI)=R(p×​RTI)=Rp ^RT​

6.3.3. 再来证明原式

6.3.3.1. 等式右侧

$\begin{aligned} &\because Rp\hat{~}R^T=(Rp)\hat{~} \\ &\therefore e^{(Rp)\hat{~}}=e^{Rp\hat{~}R^T} \approx I+Rp\hat{~}R^T \end{aligned}$​∵Rp ^RT=(Rp) ^∴e(Rp) ^=eRp ^RT≈I+Rp ^RT​

6.3.3.2. 等式左侧

$\begin{aligned} Re^{(p\hat{~})}R^T &\approx R(I+p\hat{~})R^T \\ &=RIR^T+Rp\hat{~}R^T \\ &=I+Rp\hat{~}R^T \end{aligned}$Re(p ^)RT​≈R(I+p ^)RT=RIRT+Rp ^RT=I+Rp ^RT​

6.3.3.3. 证毕

---

7. 不用SE3的讨论

8. 作业

8.1. 使用右乘SO3,求$\frac{d(R^{-1}p)}{dR}$dRd(R−1p)​

$\begin{aligned} \frac{d(R^{-1}p)}{dR}&=\lim_{\phi \to 0} \frac{d[(R\exp(\phi \hat{~}))^{-1}-R^{-1}p]}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d[\exp^{-1}(\phi \hat{~})~R^{-1}p-R^{-1}p]}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d[\exp(-\phi \hat{~})~R^{-1}p-R^{-1}p]}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d[(I-\phi \hat{~})~R^{-1}p-R^{-1}p]}{d \phi} \\ &=\lim_{\phi \to 0} \frac{-\phi\hat{~}~R^{-1}p}{\phi} \\ &=\lim_{\phi \to 0} \frac{(R^{-1}p\hat{~})\phi}{\phi} \\ &=(R^{-1}p)\hat{~} \end{aligned}$dRd(R−1p)​​=ϕ→0lim​dϕd[(Rexp(ϕ ^))−1−R−1p]​=ϕ→0lim​dϕd[exp−1(ϕ ^) R−1p−R−1p]​=ϕ→0lim​dϕd[exp(−ϕ ^) R−1p−R−1p]​=ϕ→0lim​dϕd[(I−ϕ ^) R−1p−R−1p]​=ϕ→0lim​ϕ−ϕ ^ R−1p​=ϕ→0lim​ϕ(R−1p ^)ϕ​=(R−1p) ^​

8.2. 使用右乘SO3,求$\frac{d\ln(R_1R_2^{-1})}{dR_2}$dR2​dln(R1​R2−1​)​

$\begin{aligned} \frac{d\ln(R_1R_2^{-1})}{dR_2}&=\lim_{\phi \to 0} \frac{d \ln[R_1(R_2 \exp(\phi \hat{~}))^{-1}]-ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln[R_1\exp^{-1}(\phi \hat{~})R_2^{-1}]-ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln[R_1\exp(-\phi \hat{~})R_2^{-1}]-ln(R_1R_2^{-1})}{d \phi} \end{aligned}$dR2​dln(R1​R2−1​)​​=ϕ→0lim​dϕdln[R1​(R2​exp(ϕ ^))−1]−ln(R1​R2−1​)​=ϕ→0lim​dϕdln[R1​exp−1(ϕ ^)R2−1​]−ln(R1​R2−1​)​=ϕ→0lim​dϕdln[R1​exp(−ϕ ^)R2−1​]−ln(R1​R2−1​)​​
再利用伴随性质
$\begin{aligned} R\exp(\phi \hat{~} )R^T=\exp((R\phi)\hat{~}) \end{aligned}$Rexp(ϕ ^)RT=exp((Rϕ) ^)​
可得
$\begin{aligned} R_2\exp(-\phi \hat{~})R_2^T=\exp((R_2*(-\phi))\hat{~}) \end{aligned}$R2​exp(−ϕ ^)R2T​=exp((R2​∗(−ϕ)) ^)​
两边同时左乘$R_2^{-1}$R2−1​
$\begin{aligned} \exp(-\phi \hat{~})R_2^{-1}=R_2^{-1}\exp((R_2*(-\phi))\hat{~}) \end{aligned}$exp(−ϕ ^)R2−1​=R2−1​exp((R2​∗(−ϕ)) ^)​
将上式带入
$\begin{aligned} \frac{d\ln(R_1R_2^{-1})}{dR_2}&=\lim_{\phi \to 0} \frac{d \ln[R_1(R_2 \exp(\phi \hat{~}))^{-1}]-\ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln[R_1\exp^{-1}(\phi \hat{~})R_2^{-1}]-\ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln[R_1\exp(-\phi \hat{~})R_2^{-1}]-\ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln[R_1R_2^{-1}\exp((R_2*(-\phi))\hat{~})]-\ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln(R_1R_2^{-1})+J_r^{-1}R_2*(-\phi)-\ln(R_1R_2^{-1})}{d \phi} \\ &=\lim_{\phi \to 0} \frac{d \ln(R_1R_2^{-1})-J_r^{-1}\phi R_2-\ln(R_1R_2^{-1})}{d \phi} \\ &=-J_r^{-1}(\ln(R_1R_2^{-1}))R_2 \end{aligned}$dR2​dln(R1​R2−1​)​​=ϕ→0lim​dϕdln[R1​(R2​exp(ϕ ^))−1]−ln(R1​R2−1​)​=ϕ→0lim​dϕdln[R1​exp−1(ϕ ^)R2−1​]−ln(R1​R2−1​)​=ϕ→0lim​dϕdln[R1​exp(−ϕ ^)R2−1​]−ln(R1​R2−1​)​=ϕ→0lim​dϕdln[R1​R2−1​exp((R2​∗(−ϕ)) ^)]−ln(R1​R2−1​)​=ϕ→0lim​dϕdln(R1​R2−1​)+Jr−1​R2​∗(−ϕ)−ln(R1​R2−1​)​=ϕ→0lim​dϕdln(R1​R2−1​)−Jr−1​ϕR2​−ln(R1​R2−1​)​=−Jr−1​(ln(R1​R2−1​))R2​​

8.3. CODEing

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

using namespace Eigen;
using namespace std;

int main()
{
    //旋转向量
    Eigen::AngleAxisd angleAxisVec=AngleAxisd(M_PI/2,Vector3d(0,0,1));
    //Eigen 旋转矩阵
    Eigen::Matrix3d R_mat=angleAxisVec.toRotationMatrix();
    cout<<"Eigen 绕z轴旋转90度的旋转矩阵"<<R_mat<<endl;

    /**********************************************************
     * 当有一个计算出来的w,使用它来对旋转量进行更新
     * 1.使用旋转矩阵 R=R*exp(w^)
     * 2.使用四元数  q=q * q[1,0.5w]
     * 结果:这两种更新方式都是一样的
     **********************************************************/
    ///1.使用旋转矩阵更新
    Sophus::SO3d R;
    cout<<"更新之前的R: \n"<<R.matrix()<<endl;
    Vector3d w(0.01,0.02,0.03);
//    cout<<"更新量w的反对称矩阵:\n"<<Sophus::SO3d::hat(w)<<endl;
//    Matrix3d hat=R.matrix()*Sophus::SO3d::hat(w);   //这个并不是更新之后的R
    Sophus::SO3d R_updated_sophus=R*Sophus::SO3d::exp(w);
    cout<<"更新之后的R: \n"<<R_updated_sophus.matrix()<<endl;

    cout<<"======================================================"<<endl;
    ///2.使用四元数更新
    Quaterniond q(R.matrix());
    cout<<"更新之前的q: \n"<<q.coeffs().transpose()<<endl;
    cout<<"对应的旋转矩阵: \n"<<q.toRotationMatrix()<<endl;

    //注意:这里不是用旋转向量构造q, w是直接构造q的参数
//    Quaterniond q_update(Eigen::AngleAxisd(1,Vector3d(0.5*0.01,0.5*0.02,0.5*0.03)));

    //注意两种构造方式
//    Quaterniond(Vector4d(x,y,z,w))
//    Quaterniond(w,x,y,z,w)
    Quaterniond q_update(1,0.5*0.01,0.5*0.02,0.5*0.03);  //构造更新量四元数q(w,x,y,z)
//    cout<<q_update.coeffs().transpose()<<endl;
    Quaterniond q_updated=q*q_update;
    cout<<"更新之后的q对应的旋转矩阵:\n"<<q_updated.toRotationMatrix()<<endl;
    return 0;
}

8.3.1. Result