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

在这里插入图片描述

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