VSLAM::[手寫VIO_課堂筆記]第一講_預備知識

在這裏插入圖片描述

1. 四元數的基本運算

  • 主要運算
    在這裏插入圖片描述
  • 四元數乘法

乘法性質

  1. 滿足結合律
  2. 不滿足交換律
  3. 乘積的模等於模的乘積
  4. 乘積的逆等於各個四元數的逆以相反的順序相乘
  • 其他運算
    在這裏插入圖片描述
    在這裏插入圖片描述

*四元數部分參考:旋轉矩陣、歐拉角、四元數理論及其轉換關係


2. 四元數與旋轉向量

在這裏插入圖片描述
在這裏插入圖片描述
在這裏插入圖片描述

簡單來說,四元數的思想就是把方向餘弦矩陣的三次旋轉表示爲只繞一個旋轉軸旋轉一次完成,因此可以用4個數來表示這個過程,其中包括旋轉軸向量的長度(θ)和旋轉軸單位向量(x,y,z)

2.1. 四元數的表示

[q0q1q2q3]=[cosθ2xsinθ2ysinθ2zsinθ2] \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}


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. 即證明下式

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

6.3.2. 首先證明

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

6.3.2.1. 已知


a=b×c \begin{aligned} a=b\times c \end{aligned}
則有
RSO3Ra=(Rb)×(Rc) \begin{aligned} &R\in SO3 \\ &Ra=(Rb) \times (Rc) \end{aligned}

6.3.2.2. 可證

(Rp) ^=(Rp) ^I=(Rp)×RRTI=(Rp)×(RRTI)=R(p×RTI)=Rp ^RT \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}

6.3.3. 再來證明原式

6.3.3.1. 等式右側

Rp ^RT=(Rp) ^e(Rp) ^=eRp ^RTI+Rp ^RT \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}

6.3.3.2. 等式左側

Re(p ^)RTR(I+p ^)RT=RIRT+Rp ^RT=I+Rp ^RT \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}

6.3.3.3. 證畢

7. 不用SE3的討論

在這裏插入圖片描述

8. 作業

在這裏插入圖片描述

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

d(R1p)dR=limϕ0d[(Rexp(ϕ ^))1R1p]dϕ=limϕ0d[exp1(ϕ ^) R1pR1p]dϕ=limϕ0d[exp(ϕ ^) R1pR1p]dϕ=limϕ0d[(Iϕ ^) R1pR1p]dϕ=limϕ0ϕ ^ R1pϕ=limϕ0(R1p ^)ϕϕ=(R1p) ^ \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}

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

dln(R1R21)dR2=limϕ0dln[R1(R2exp(ϕ ^))1]ln(R1R21)dϕ=limϕ0dln[R1exp1(ϕ ^)R21]ln(R1R21)dϕ=limϕ0dln[R1exp(ϕ ^)R21]ln(R1R21)dϕ \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}
再利用伴隨性質
Rexp(ϕ ^)RT=exp((Rϕ) ^) \begin{aligned} R\exp(\phi \hat{~} )R^T=\exp((R\phi)\hat{~}) \end{aligned}
可得
R2exp(ϕ ^)R2T=exp((R2(ϕ)) ^) \begin{aligned} R_2\exp(-\phi \hat{~})R_2^T=\exp((R_2*(-\phi))\hat{~}) \end{aligned}
兩邊同時左乘R21R_2^{-1}
exp(ϕ ^)R21=R21exp((R2(ϕ)) ^) \begin{aligned} \exp(-\phi \hat{~})R_2^{-1}=R_2^{-1}\exp((R_2*(-\phi))\hat{~}) \end{aligned}
將上式帶入
dln(R1R21)dR2=limϕ0dln[R1(R2exp(ϕ ^))1]ln(R1R21)dϕ=limϕ0dln[R1exp1(ϕ ^)R21]ln(R1R21)dϕ=limϕ0dln[R1exp(ϕ ^)R21]ln(R1R21)dϕ=limϕ0dln[R1R21exp((R2(ϕ)) ^)]ln(R1R21)dϕ=limϕ0dln(R1R21)+Jr1R2(ϕ)ln(R1R21)dϕ=limϕ0dln(R1R21)Jr1ϕR2ln(R1R21)dϕ=Jr1(ln(R1R21))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}

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

在這裏插入圖片描述

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章