精华内容
下载资源
问答
  • 最小二乘问题解决方法
    2021-10-17 21:12:41

    最小二乘问题

    最小二乘问题(Least Squares Problem),即

    min ⁡ x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \min_{\mathbf{x}} \mathbf{F}(\mathbf{x}) = \frac{1}{2} {\|\mathbf{f}(\mathbf{x})\|}^2_2 xminF(x)=21f(x)22

    其中, F ( x ) \mathbf{F}(\mathbf{x}) F(x)损失函数 f ( x ) \mathbf{f}(\mathbf{x}) f(x)残差函数,定义为

    F ( x ) = 1 2 ∑ i = 1 m ( f i ( x ) ) 2 = 1 2 f ⊤ ( x ) f ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \mathbf{F}(\mathbf{x}) = \frac{1}{2} \sum_{i=1}^{m}\left(f_{i}(\mathbf{x})\right)^{2} = \frac{1}{2} \mathbf{f}^{\top}(\mathbf{x}) \mathbf{f}(\mathbf{x}) = \frac{1}{2} {\|\mathbf{f}(\mathbf{x})\|}^2_2 F(x)=21i=1m(fi(x))2=21f(x)f(x)=21f(x)22

    • 若 残差函数 f ( x ) \mathbf{f}(\mathbf{x}) f(x) 为 线性函数,则这个问题即为 线性最小二乘问题
    • 若 残差函数 f ( x ) \mathbf{f}(\mathbf{x}) f(x) 为 非线性函数,则这个问题即为 非线性最小二乘问题

    线性最小二乘问题

    min ⁡ x ∥ A x − b ∥ \min_{x} \|Ax-b\| xminAxb

    • 正规方程组

    A T A x = A T b A^TA x = A^T b ATAx=ATb

    • QR分解

    min ⁡ x ∥ A x − b ∥ = min ⁡ x ∥ Q R x − b ∥ = min ⁡ x ∥ R x − Q T b ∥ \min_{x} \|Ax-b\| = \min_{x} \|QRx-b\| = \min_{x} \|Rx - Q^T b\| xminAxb=xminQRxb=xminRxQTb

    • 乔列斯基分解

    非线性最小二乘问题

    对于不方便求解的最小二乘问题,我们用 迭代 的方式,从一个初始值出发,不断地更新当前的优化变量,使目标函数下降。这让求解 导函数为零 的问题变成了一个不断 寻找下降增量 Δ x \Delta \mathbf{x} Δx 的问题。

    1. 给定某个初始值 x 0 \mathbf{x}_0 x0
    2. 对于第k次迭代,寻找一个增量 Δ x k \Delta \mathbf{x}_k Δxk,使得 ∥ f ( x k + Δ x k ) ∥ 2 2 {\|f(\mathbf{x}_k + \Delta \mathbf{x}_k)\|}_2^2 f(xk+Δxk)22 达到极小值
    3. Δ x k \Delta \mathbf{x}_k Δxk 足够小,则停止
    4. 否则,令 x k + 1 = x k + Δ x k \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta \mathbf{x}_k xk+1=xk+Δxk,返回第2步

    实际上,非线性优化的所有迭代求解方案,都需要用户提供一个 良好的初始值

    损失函数 线性化

    F ( x ) \mathbf{F}(\mathbf{x}) F(x) 进行线性化,对其进行 二阶泰勒展开,得

    F ( x + Δ x ) = F ( x ) + Δ x ⊤ F ′ ( x ) + 1 2 Δ x ⊤ F ′ ′ ( x ) Δ x + O ( ∥ Δ x ∥ 3 ) \mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) = \mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{F}^{\prime}(\mathbf{x}) + \frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{F}^{\prime\prime}(\mathbf{x}) \Delta \mathbf{x} + O \left(\|\Delta \mathbf{x}\|^{3}\right) F(x+Δx)=F(x)+ΔxF(x)+21ΔxF(x)Δx+O(Δx3)

    g = F ′ ( x ) = f ′ ( x ) ⊤ f ( x ) H = F ′ ′ ( x ) \begin{aligned} \mathbf{g} &= \mathbf{F}^{\prime}(\mathbf{x}) = \mathbf{f}^{\prime}(\mathbf{x})^\top \mathbf{f}(\mathbf{x}) \\ \mathbf{H} &= \mathbf{F}^{\prime\prime}(\mathbf{x}) \end{aligned} gH=F(x)=f(x)f(x)=F(x)

    忽略高阶余项,损失函数变成了二次函数,可以轻易得到如下性质:

    • 如果在点 x s \mathbf{x}_s xs 处有 g s \mathbf{g}_s gs 为 0(对应 H s \mathbf{H}_s Hs),则称这个点为 驻点(stationary point)
      • 如果 H s \mathbf{H}_s Hs 是正定矩阵,则 x s \mathbf{x}_s xs局部最小值点(local minimizer)
      • 如果 H s \mathbf{H}_s Hs 是负定矩阵,则 x s \mathbf{x}_s xs局部最大值点(local maximizer)
      • 如果 H s \mathbf{H}_s Hs 是不定矩阵,则 x s \mathbf{x}_s xs鞍点(saddle point)

    对于泰勒展开,

    • 保留 一阶项,求解方法为 一阶梯度法
    • 保留 二阶项,求解方法为 二阶梯度法

    一阶梯度法(最速下降法)

    保留 泰勒展开 一阶项

    F ( x + Δ x ) = F ( x ) + Δ x ⊤ g \mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) = \mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{g} F(x+Δx)=F(x)+Δxg

    梯度的负方向为最速下降方向,增量方程 为

    Δ x s d = − F ′ ( x ) = − g \Delta \mathbf{x}_{\mathrm{sd}} = - \mathbf{F}^{\prime}(\mathbf{x}) = - \mathbf{g} Δxsd=F(x)=g

    • 适用于迭代的开始阶段
    • 缺点:最优值附近震荡,收敛慢

    二阶梯度法(牛顿法)

    保留 泰勒展开 二阶项

    F ( x + Δ x ) = F ( x ) + Δ x ⊤ g + 1 2 Δ x ⊤ H Δ x \mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) = \mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{g} + \frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{H} \Delta \mathbf{x} F(x+Δx)=F(x)+Δxg+21ΔxHΔx

    F ( x + Δ x ) \mathbf{F}(\mathbf{x} + \Delta \mathbf{x}) F(x+Δx) 关于 Δ x \Delta \mathbf{x} Δx 的导数并令其为零,即

    F ′ ( x + Δ x ) = 0 ⟹ g + H Δ x = 0 ⟹ H Δ x = − g \mathbf{F}^{\prime}(\mathbf{x} + \Delta \mathbf{x}) = 0 \quad \Longrightarrow \quad \mathbf{g} + \mathbf{H} \Delta \mathbf{x} = 0 \quad \Longrightarrow \quad \mathbf{H} \Delta \mathbf{x} = - \mathbf{g} F(x+Δx)=0g+HΔx=0HΔx=g

    则,增量方程 为

    Δ x n = − H − 1 g \Delta \mathbf{x}_{\mathrm{n}} = -\mathbf{H}^{-1} \mathbf{g} Δxn=H1g

    • 适用于最优值附近
    • 缺点:二阶导矩阵 H \mathbf{H} H 计算复杂

    残差函数 线性化

    f ( x ) \mathbf{f}(\mathbf{x}) f(x) 进行线性化,对其进行 一阶泰勒展开,得

    f ( x + Δ x ) ≈ ℓ ( Δ x ) ≡ f ( x ) + J ( x ) Δ x with J ( x ) = f ′ ( x ) \mathbf{f}(\mathbf{x}+\Delta \mathbf{x}) \approx \ell(\Delta \mathbf{x}) \equiv \mathbf{f}(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \Delta \mathbf{x} \quad \text{with} \quad \mathbf{J}(\mathbf{x}) = \mathbf{f}^{\prime}(\mathbf{x}) f(x+Δx)(Δx)f(x)+J(x)ΔxwithJ(x)=f(x)

    代入 损失函数

    F ( x + Δ x ) ≈ L ( Δ x ) ≡ 1 2 ℓ ( Δ x ) ⊤ ℓ ( Δ x ) = 1 2 f ⊤ f + Δ x ⊤ J ⊤ f + 1 2 Δ x ⊤ J ⊤ J Δ x = F ( x ) + Δ x ⊤ J ⊤ f + 1 2 Δ x ⊤ J ⊤ J Δ x \begin{aligned} \mathbf{F}(\mathbf{x}+\Delta \mathbf{x}) \approx \mathbf{L}(\Delta \mathbf{x}) & \equiv \frac{1}{2} \ell(\Delta \mathbf{x})^{\top} \ell(\Delta \mathbf{x}) \\ &=\frac{1}{2} \mathbf{f}^{\top} \mathbf{f} + \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \\ &=\mathbf{F}(\mathbf{x}) + \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{f}+\frac{1}{2} \Delta \mathbf{x}^{\top} \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} \end{aligned} F(x+Δx)L(Δx)21(Δx)(Δx)=21ff+ΔxJf+21ΔxJJΔx=F(x)+ΔxJf+21ΔxJJΔx

    并且

    g = F ′ ( x ) = J ⊤ f H = F ′ ′ ( x ) ≈ J ⊤ J \begin{aligned} \mathbf{g} &= \mathbf{F}^{\prime}(\mathbf{x}) = \mathbf{J}^\top \mathbf{f} \\ \mathbf{H} &= \mathbf{F}^{\prime\prime}(\mathbf{x}) \approx \mathbf{J}^{\top} \mathbf{J} \end{aligned} gH=F(x)=Jf=F(x)JJ

    高斯-牛顿法(G-N)

    L ( Δ x ) \mathbf{L}(\Delta \mathbf{x}) L(Δx) 关于 Δ x \Delta \mathbf{x} Δx 的导数并令其为零,即

    L ′ ( Δ x ) = 0 ⟹ J ⊤ f + J ⊤ J Δ x = 0 \mathbf{L}^{\prime}(\Delta \mathbf{x}) = 0 \quad \Longrightarrow \quad \mathbf{J}^{\top} \mathbf{f} + \mathbf{J}^{\top} \mathbf{J} \Delta \mathbf{x} = 0 L(Δx)=0Jf+JJΔx=0

    则,增量方程 为

    ( J ⊤ J ) Δ x g n = − J ⊤ f \left(\mathbf{J}^{\top} \mathbf{J}\right) \Delta \mathbf{x}_{\mathrm{gn}}=-\mathbf{J}^{\top} \mathbf{f} (JJ)Δxgn=Jf

    简写为

    H Δ x g n = − g with H ≈ J ⊤ J \mathbf{H} \Delta \mathbf{x}_{\mathrm{gn}} = -\mathbf{g} \quad \text{with} \quad \mathbf{H} \approx \mathbf{J}^{\top} \mathbf{J} HΔxgn=gwithHJJ

    • J ⊤ J \mathbf{J}^{\top} \mathbf{J} JJ 可能为奇异矩阵或病态的情况,此时增量的稳定性较差,导致算法不收敛

    列文伯格-马夸尔特法(L-M)

    ( J ⊤ J + μ I ) Δ x lm ⁡ = − J ⊤ f  with  μ ≥ 0 \left(\mathbf{J}^{\top} \mathbf{J}+\mu \mathbf{I}\right) \Delta \mathbf{x}_{\operatorname{lm}}=-\mathbf{J}^{\top} \mathbf{f} \quad \text { with } \mu \geq 0 (JJ+μI)Δxlm=Jf with μ0

    鲁棒核函数(M估计)

    • Huber
    • Cauchy

    在前面的非线性最小二乘问题中,我们最小化误差项的二范数平方和,作为目标函数。当出现误匹配时,误差 e e e 就会很大,如果我们再使用平方,这个误差就会更大,算法将试图调整这条边所连接的节点的估计值,使它们顺应这条边的无理要求。由于这个边的误差真的很大,往往会抹平了其他正确边的影响,使优化算法专注于调整一个错误的值。

    出现这种问题的原因是,当误差很大时,二范数增长得太快了。于是就有了核函数的存在。 核函数保证每条边的误差不会太大以至于掩盖掉其他的边。这种核函数称之为鲁棒核函数。

    最常用的鲁棒核函数有Huber核:

    H ( e ) = { 1 2 e 2                 if  ∣ e ∣ < δ , δ ( ∣ e ∣ − 1 2 δ )     otherwise H(e)= \left\{ \begin{array}{lr} \frac{1}{2}e^2\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \text{if}\ |e|<\delta ,\\ \delta(|e|-\frac{1}{2}\delta) \ \ \ \ \text{otherwise} \end{array} \right. H(e)={21e2                if e<δ,δ(e21δ)    otherwise

    当误差 e e e 大于某个阈值 δ \delta δ 后,函数增长由二次形式变成了一次形式,相当于限制了梯度的最大值。同时,Huber 核函数又是光滑的,可以很方便地求导。

    更多相关内容
  • 1.累加累乘与积分 1.将向量下标为偶数的分量 (x2, x4, …) 累加, 写出相应...∑xij≥1xij2\sum_{x_{ij}≥1}x_{ij}^2xij​≥1∑​xij2​ ②将矩阵中不为0的分量累乘: ∏xij≠0xij\prod_{x_{ij}≠0}x_{ij}xij​​=

    1.累加累乘与积分

    1. 将向量下标为偶数的分量累加, 写出相应表达式.
      ∑ i m o d    2 = 0 x i \sum_{i \mod 2=0} x_i imod2=0xi
    2. 各出一道累加、累乘、积分表达式的习题, 并给出标准答案.
      ①将矩阵中大于等于1的分量平方并累加:
      ∑ x i j ≥ 1 x i j 2 \sum_{x_{ij}≥1}x_{ij}^2 xij1xij2
      ②将矩阵中不为0的分量累乘:
      ∏ x i j ≠ 0 x i j \prod_{x_{ij}≠0}x_{ij} xij=0xij
      ③函数 x 3 − x + 2 x^3-x+2 x3x+2在0到1上积分:
      ∫ 0 1 x 3 − x + 2 d x \int_{0}^{1}x^3-x+2 \mathrm{d}x 01x3x+2dx
    3. 你使用过三重累加吗? 描述一下其应用.
      在代码中三重循环算时间复杂度时会用到三重累加: ∑ i = 1 n ∑ j = 1 i ∑ k = 1 j 1 \sum_{i=1}^n\sum_{j=1}^i\sum_{k=1}^j 1 i=1nj=1ik=1j1
    4. 给一个常用的定积分, 将手算结果与程序结果对比.
      定积分: ∫ 0 1 x 2 + 1 d x \int_{0}^{1}x^2+1 \mathrm{d}x 01x2+1dx
      手算结果: 4 3 = 1.33333... \frac{4}{3}=1.33333... 34=1.33333...
      程序结果: 1.32835 1.32835 1.32835
      在这里插入图片描述

    2.线性回归

    1. 如何获得 w \mathbf{w} w?
      推导过程:
      ∥ X w − Y ∥ 2 2 = ( X w − Y ) T ( X w − Y ) = ( w T X T − Y T ) ( X w − Y ) = w T X T X w − w T X T Y − Y T X w + Y T Y \begin{aligned} \|\mathbf{Xw}-\mathbf{Y}\|_2^{2} & =(\mathbf{Xw}-\mathbf{Y})^\mathrm{T}(\mathbf{Xw}-\mathbf{Y}) \\ & =(\mathbf{w}^\mathrm{T}\mathbf{X}^\mathrm{T}-\mathbf{Y}^\mathrm{T})(\mathbf{Xw}-\mathbf{Y}) \\ & =\mathbf{w}^\mathrm{T}\mathbf{X}^\mathrm{T}\mathbf{Xw}-\mathbf{w}^\mathrm{T}\mathbf{X}^\mathrm{T}\mathbf{Y}- \mathbf{Y}^\mathrm{T}\mathbf{Xw}+\mathbf{Y}^\mathrm{T}\mathbf{Y} \end{aligned} XwY22=(XwY)T(XwY)=(wTXTYT)(XwY)=wTXTXwwTXTYYTXw+YTY
      将该式关于 w \mathbf{w} w求导 (使用向量求导法则) 并令其为 0, 可得
      X T X w − X T Y = 0 \mathbf{X}^{\mathrm{T}}\mathbf{X} \mathbf{w} - \mathbf{X}^{\mathrm{T}}\mathbf{Y} = 0 XTXwXTY=0
      最后
      w = ( X T X ) − 1 X T Y \mathbf{w} = (\mathbf{X}^{\mathrm{T}}\mathbf{X})^{-1}\mathbf{X}^{\mathrm{T}}\mathbf{Y} w=(XTX)1XTY
      岭回归:
      arg min ⁡ w = ∥ X w − Y ∥ 2 2 − λ ∥ w ∥ 2 2 \argmin_{\mathbf{w}}=\|\mathbf{Xw}-\mathbf{Y}\|_2^2-λ\|\mathbf{w}\|_2^2 wargmin=XwY22λw22
      可推导出
      X T X w − X T Y + λ w = 0 \mathbf{X}^{\mathrm{T}}\mathbf{X} \mathbf{w} - \mathbf{X}^{\mathrm{T}}\mathbf{Y} + \lambda \mathbf{w} = 0 XTXwXTY+λw=0
      最后
      w = ( X T X + λ I ) − 1 X T Y \mathbf{w} = (\mathbf{X}^{\mathrm{T}}\mathbf{X} + \lambda \mathbf{I})^{-1}\mathbf{X}^{\mathrm{T}}\mathbf{Y} w=(XTX+λI)1XTY

    2. 自己写一个小例子 ( n = 3 , m = 1 ) (n=3, m = 1) (n=3,m=1) 来验证最小二乘法.
      令函数 y = 2 x + 1 y=2x+1 y=2x+1, x x x取值5, 6, 7, y ^ \hat y y^取值10, 13, 14.
      y ^ = a x + b \hat y=ax+b y^=ax+b, 用最小二乘法求 a , b a,b a,b :
      a = ∑ i = 1 3 x i y ^ i − 3 x ‾ y ‾ ∑ i = 1 3 x i 2 − 3 x ‾ 2 = 2.028 a=\frac{\sum_{i=1}^3x_i\hat y_i-3\overline{x}\overline{y}}{\sum_{i=1}^3x_i^2-3\overline x^2}=2.028 a=i=13xi23x2i=13xiy^i3xy=2.028
      b = y ‾ − a x ‾ = 0.132 b=\overline{y}-a\overline{x}=0.132 b=yax=0.132

    3.推导, 并描述这个方法的特点 (不少于 5 条).

    arg max ⁡ w L ( x ) = ∏ i = 1 n P ( y i ∣ x i ; w ) \argmax_\mathbf{w} L(x)=\prod_{i=1}^nP(y_i|\mathbf{x}_i; \mathbf{w}) wargmaxL(x)=i=1nP(yixi;w)

    • 相乘计算困难, 将其求一个对数, 不改变单调性
      log ⁡ L ( w ) = ∑ i = 1 n log ⁡ P ( y i ∣ x i ; w ) = ∑ i = 1 n y i log ⁡ P ( y i = 1 ∣ x i ; w ) + ( 1 − y i ) log ⁡ ( 1 − P ( y i = 1 ∣ x i ; w ) ) = ∑ i = 1 n y i log ⁡ P ( y i = 1 ∣ x i ; w ) 1 − P ( y i = 1 ∣ x i ; w ) + log ⁡ ( 1 − P ( y i = 1 ∣ x i ; w ) ) = ∑ i = 1 n y i x i w − log ⁡ ( 1 + e x i w ) \begin{aligned}\log L(\mathbf{w}) & = \sum_{i = 1}^n \log P(y_i \vert \mathbf{x}i; \mathbf{w}) \\ & = \sum_{i = 1}^n y_i \log P(y_i = 1 \vert \mathbf{x}_i; \mathbf{w}) + (1 - y_i) \log(1 - P(y_i = 1 \vert \mathbf{x}i; \mathbf{w})) \\ & = \sum_{i = 1}^n y_i \log \frac{P(y_i = 1 \vert \mathbf{x}_i; \mathbf{w})}{1 - P(y_i = 1 \vert \mathbf{x}_i; \mathbf{w})} + \log (1 - P(y_i = 1 \vert \mathbf{x}i; \mathbf{w})) \\ & = \sum_{i = 1}^n y_i \mathbf{x}_i \mathbf{w} - \log (1 + e^{\mathbf{x}_i \mathbf{w}}) \end{aligned} logL(w)=i=1nlogP(yixi;w)=i=1nyilogP(yi=1xi;w)+(1yi)log(1P(yi=1xi;w))=i=1nyilog1P(yi=1xi;w)P(yi=1xi;w)+log(1P(yi=1xi;w))=i=1nyixiwlog(1+exiw)
    • w \mathbf{w} w 求偏导
      ∂ log ⁡ L ( w ) ∂ w = ∑ i = 1 n y i x i − e x i w 1 + e x i w x i = ∑ i = 1 n ( y i − e x i w 1 + e x i w ) x i \begin{aligned} \frac{\partial \log L(\mathbf{w})}{\partial \mathbf{w}} & = \sum_{i = 1}^n y_i \mathbf{x}_i - \frac{e^{\mathbf{x}_i \mathbf{w}}}{1 + e^{\mathbf{x}_i \mathbf{w}}} \mathbf{x}_i \\ & = \sum_{i = 1}^n \left(y_i - \frac{e^{\mathbf{x}_i \mathbf{w}}}{1 + e^{\mathbf{x}_i \mathbf{w}}}\right) \mathbf{x}_i \end{aligned} wlogL(w)=i=1nyixi1+exiwexiwxi=i=1n(yi1+exiwexiw)xi
    • 令该偏导为 0, 无法获得解析式, 因此用梯度下降.
      w t + 1 = w t − α ∂ log ⁡ L ( w ) ∂ w \mathbf{w}^{t+1}=\mathbf{w}^{t}-\alpha\frac{\partial \log L(\mathbf{w})}{\partial \mathbf{w}} wt+1=wtαwlogL(w)
    1. sigmoid函数更符合分类问题;
    2. 将累乘变换为累加;
    3. 输出为概率形式;
    4. sigmoid函数任意阶可导;
    5. 特征离散化,增加泛化能力.
    展开全文
  • 李群与李代数2:李代数求导和李群扰动模型1. 整体误差最小化引出求导问题2. BCH公式与近似形式3. 李代数求导 前言:本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》的第四讲,讲解李群与李代数。写此篇的...


    前言:本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》的第四讲,讲解李群与李代数。写此篇的目的是为了补足《十四讲》中的数学类基础知识,原书内容有点像直译文献,因此笔者根据自己理解,对整体内容重新划分,更改了目录章节,改写部分公式,同时扩展了部分内容,有书的同学可以参照阅读,达到事半功倍的效果。

    1. 整体误差最小化引出求导问题

    使用李代数的一大动机是进行优化,而在优化过程中导数是非常必要的信息。下面来讨论一个带有李代数的函数,以及关于该李代数求导的问题,该问题有很强的实际应用背景。在SLAM中,我们使用 S O ( 3 ) SO(3) SO(3)上的旋转矩阵或 S E ( 3 ) SE(3) SE(3)上的变换矩阵来描述如何估计一个相机的位置和姿态(位姿)。不妨设某个时刻机器人的位姿为 T \boldsymbol{T} T,它观察到一个世界坐标位于 p p p的点,产生了一个观测数据 z z z。那么,由坐标变换关系知: z = T p + w . (1.1) z=\boldsymbol{T}p+w.\tag{1.1} z=Tp+w.(1.1)其中, w w w为随机噪声。由于它的存在, z z z往往不可能精确地满足 z = T p z=Tp z=Tp的关系。所以,我们通常会计算理想的观测与实际数据的误差: e = z − T p . (1.2) e=z-\boldsymbol{T}p.\tag{1.2} e=zTp.(1.2)假设一共有 N N N个这样的路标点和观测,于是就有 N N N个上式。那么,对机器人进行位姿估计,相当于找到一个最优的 T \boldsymbol{T} T,使得整体误差最小化: min ⁡ T J ( T ) = ∑ i = 1 N ∥ z i − T p i ∥ 2 . (1.3) \min_{\boldsymbol{T}}J(\boldsymbol{T})=\sum_{i=1}^{N}\left \| z_{i}-\boldsymbol{T}p_{i} \right \|^{2}.\tag{1.3} TminJ(T)=i=1NziTpi2.(1.3)求解此问题,需要计算目标函数 J J J关于变换矩阵 T \boldsymbol{T} T的导数。我们把具体的算法留到后面讲(精力有限,本系列博客不再涉及,感兴趣的同学去读原书吧)。这里的重点是,我们经常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。然而, S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)上并没有良好定义的加法,它们只是群,如果我们把 T \boldsymbol{T} T当成一个普通矩阵来处理优化,就必须对它加以约束,就会比较繁琐。
    而从李代数角度来说,由于李代数由向量组成,具有良好的加法运算,因此,可以使用李代数解决求导问题,求导思路分为两种:

    1. 用李代数表示姿态,然后根据李代数加法对李代数求导。
    2. 对李群左乘右乘微小扰动,然后对该扰动对应的李代数求导,称为李群左扰动或右扰动模型。

    第一种方式对应到李代数的求导,而第二种方式则对应到扰动模型。在讨论这两种思路的异同之前,我们先看一下BCH(Baker-Campbell-Hausdorff)线性近似。

    2. BCH公式与近似形式

    2.1 BCH公式

    下面来考虑一个问题。虽然我们已经清楚了 S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3)上的李群和李代数关系,但是当在 S O ( 3 ) SO(3) SO(3)中完成两个矩阵乘法时,李代数中 s e ( 3 ) \mathfrak{se}(3) se(3)上发生了什么改变呢?反过来说,当 s o ( 3 ) \mathfrak{so}(3) so(3)做两个李代数的加法时, S O ( 3 ) SO(3) SO(3)上是否对应着两个矩阵的乘积?如果成立,相当于: exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) = exp ⁡ ( ( ϕ 1 + ϕ 2 ) ∧ ) \exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2})=\exp((\phi_{1}+\phi_{2})^{\wedge}) exp(ϕ1)exp(ϕ2)=exp((ϕ1+ϕ2))如果 ϕ 1 , ϕ 2 \phi_{1},\phi_{2} ϕ1,ϕ2为标量,那么显然该式成立;但此处我们计算的是矩阵的指数函数,而非标量的指数。换言之,我们在研究下式是否成立: ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B ? \ln(\exp(A)\exp(B))=A+B? ln(exp(A)exp(B))=A+B?很遗憾,该式在矩阵时并不成立。两个李代数指数映射乘积的完整形式,由Baker-Cambell-Hausdorff公式(BCH公式)1给出。由于其完整形式较复杂,我们只给出其展开式的前几项: ln ⁡ ( exp ⁡ ( A ) exp ⁡ ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] − 1 24 [ B , [ A , [ A , B ] ] ] + ⋯ = A + B + 1 2 ( A B − B A ) + 1 12 ( A 2 B + A B 2 − 2 A B A + B 2 A + B A 2 − 2 B A B ) + 1 24 ( A 2 B 2 − 2 A B A B − B 2 A 2 + 2 B A B A ) . (2.1) \begin{aligned} \ln(\exp(A)\exp(B)) &= A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]-\frac{1}{24}[B,[A,[A,B]]]+\cdots \\&= A+B+\frac{1}{2}(AB-BA)+\frac{1}{12}(A^{2}B+AB^{2}-2ABA+B^{2}A+BA^{2}-2BAB) + \\& \quad\quad\frac{1}{24}(A^{2}B^{2}-2ABAB-B^{2}A^{2}+2BABA) \end{aligned}.\tag{2.1} ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]121[B,[A,B]]241[B,[A,[A,B]]]+=A+B+21(ABBA)+121(A2B+AB22ABA+B2A+BA22BAB)+241(A2B22ABABB2A2+2BABA).(2.1)其中[]为李括号。BCH公式告诉我们,当处理两个矩阵指数之积时,它们会产生一些由李括号组成的余项。

    2.2 BCH线性近似

    特别的,考虑 S O ( 3 ) SO(3) SO(3)上的李代数 ( ln ⁡ exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ (\ln\exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2}))^{\vee} (lnexp(ϕ1)exp(ϕ2)),当 ϕ 1 \phi_{1} ϕ1 ϕ 2 \phi_{2} ϕ2为小量时,小量二次以上的项可以被忽略,而旋转或变换更新的基本都是小量,因此BCH具有实际应用意义。此时,BCH拥有线性近似表达2

    ( ln ⁡ exp ⁡ ( ϕ 1 ∧ ) exp ⁡ ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 当 ϕ 1 为 小 量 , J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 当 ϕ 2 为 小 量 . (2.2) (\ln\exp(\phi^{\wedge}_{1})\exp(\phi^{\wedge}_{2}))^{\vee}\approx \left\{\begin{matrix} \boldsymbol{J}_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2} \quad\quad 当\phi_{1}为小量,\\ \boldsymbol{J}_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} \quad\quad 当\phi_{2}为小量. \end{matrix}\right.\tag{2.2} (lnexp(ϕ1)exp(ϕ2)){Jl(ϕ2)1ϕ1+ϕ2ϕ1,Jr(ϕ1)1ϕ2+ϕ1ϕ2.(2.2)
    以第一个近似为例。该式告诉我们,当对一个旋转矩阵 R 2 R_{2} R2(李代数为 ϕ 2 \phi_{2} ϕ2)左乘一个微小旋转矩阵 R 1 R_{1} R1(李代数为 ϕ 1 \phi_{1} ϕ1)时,可以近似地看作,在原有的李代数 ϕ 2 \phi_{2} ϕ2上加了一项 J l ( ϕ 2 ) − 1 ϕ 1 \boldsymbol{J}_{l}(\phi_{2})^{-1}\phi_{1} Jl(ϕ2)1ϕ1。同理,第二个近似描述了右乘一个微小位移的情况。于是,李代数在BCH近似下,分成了左乘近似和右乘近似两种,在使用时我们须注意使用的是左乘模型还是右乘模型。
    本文以左乘为例。左乘BCH近似雅克比 J l \boldsymbol{J}_{l} Jl事实上就是《李群与李代数1》中的式 ( 4.6 ) (4.6) (4.6)的内容: J l = J = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ . (2.3) \boldsymbol{J}_{l}=\boldsymbol{J}=\frac{\sin\theta}{\theta}\boldsymbol{I}+(1-\frac{\sin\theta}{\theta})\boldsymbol{a}\boldsymbol{a}^{T}+\frac{1-\cos\theta}{\theta}\boldsymbol{a}^{\wedge}.\tag{2.3} Jl=J=θsinθI+(1θsinθ)aaT+θ1cosθa.(2.3)它的逆为: J l − 1 = θ 2 cot ⁡ θ 2 I + ( 1 − θ 2 cot ⁡ θ 2 ) a a T + θ 2 a ∧ . (2.4) \boldsymbol{J}_{l}^{-1}=\frac{\theta}{2}\cot\frac{\theta}{2}\boldsymbol{I}+(1-\frac{\theta}{2}\cot\frac{\theta}{2})\boldsymbol{a}\boldsymbol{a}^{T}+\frac{\theta}{2}\boldsymbol{a}^{\wedge}.\tag{2.4} Jl1=2θcot2θI+(12θcot2θ)aaT+2θa.(2.4)而右乘雅克比仅需要对自变量取负号即可: J r ( ϕ ) = J l ( − ϕ ) . (2.5) \boldsymbol{J}_{r}(\phi)=\boldsymbol{J}_{l}(-\phi).\tag{2.5} Jr(ϕ)=Jl(ϕ).(2.5)这样,我们就可以谈论李群乘法与李代数加法的关系了。

    2.3 BCH近似的意义

    为了方便读者理解,我们重新叙述BCH近似的意义。假定对某个旋转 R R R,对应的李代数为 ϕ \phi ϕ,我们给它左乘一个微小旋转,记作 Δ R \Delta R ΔR,对应的李代数为 Δ ϕ \Delta \phi Δϕ。那么,在李群上,得到的结果就是 Δ R ⋅ R \Delta R \cdot R ΔRR,而在李代数上,根据BCH近似表达公式 ( 2.2 ) (2.2) (2.2),为: J l ( ϕ ) − 1 Δ ϕ + ϕ \boldsymbol{J}_{l}(\phi)^{-1}\Delta\phi+\phi Jl(ϕ)1Δϕ+ϕ。合并起来,可以简单的写成: exp ⁡ ( Δ ϕ ∧ ) exp ⁡ ( ϕ ∧ ) = exp ⁡ ( ( J l ( ϕ ) − 1 Δ ϕ ) ∧ + ϕ ) . (2.6) \exp(\Delta\phi^{\wedge})\exp(\phi^{\wedge}) = \exp\left ( (\boldsymbol{J}_{l}(\phi)^{-1}\Delta\phi)^{\wedge}+\phi \right ).\tag{2.6} exp(Δϕ)exp(ϕ)=exp((Jl(ϕ)1Δϕ)+ϕ).(2.6)同理可得右乘扰动 R ⋅ Δ R R \cdot \Delta R RΔR对应的表达式为: exp ⁡ ( ϕ ∧ ) exp ⁡ ( Δ ϕ ∧ ) = exp ⁡ ( ( J r ( ϕ ) − 1 Δ ϕ ) ∧ + ϕ ) . (2.7) \exp(\phi^{\wedge})\exp(\Delta\phi^{\wedge}) = \exp\left ( (\boldsymbol{J}_{r}(\phi)^{-1}\Delta\phi)^{\wedge}+\phi \right ).\tag{2.7} exp(ϕ)exp(Δϕ)=exp((Jr(ϕ)1Δϕ)+ϕ).(2.7)反之,如果我们在李代数上进行加法,让一个 ϕ \phi ϕ加上 Δ ϕ \Delta \phi Δϕ,那么可以近似为李群上带左右雅克比的乘法: exp ⁡ ( ( ϕ + Δ ϕ ) ∧ ) = exp ⁡ ( ( J l ( ϕ ) Δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) = exp ⁡ ( ϕ ∧ ) exp ⁡ ( ( J r ( ϕ ) Δ ϕ ) ∧ ) . (2.8) \exp\left ( (\phi+\Delta\phi)^{\wedge} \right )=\exp\left ( (\boldsymbol{J}_{l}(\phi)\Delta\phi)^{\wedge} \right )\exp(\phi^{\wedge})=\exp(\phi^{\wedge})\exp\left ( (\boldsymbol{J}_{r}(\phi)\Delta\phi)^{\wedge} \right ).\tag{2.8} exp((ϕ+Δϕ))=exp((Jl(ϕ)Δϕ))exp(ϕ)=exp(ϕ)exp((Jr(ϕ)Δϕ)).(2.8)这就为之后李代数上做微分提供了理论基础。

    同样地,对于 S E ( 3 ) SE(3) SE(3),也有类似的BCH近似: exp ⁡ ( Δ ξ ∧ ) exp ⁡ ( ξ ∧ ) ≈ exp ⁡ ( ( ȷ l − 1 Δ ξ + ξ ) ∧ ) . (2.9) \exp(\Delta\xi^{\wedge})\exp(\xi^{\wedge})\approx \exp\left ( (\mathbf{\jmath} ^{-1}_{l}\Delta\xi+\xi )^{\wedge}\right ).\tag{2.9} exp(Δξ)exp(ξ)exp((ȷl1Δξ+ξ)).(2.9) exp ⁡ ( ξ ∧ ) exp ⁡ ( Δ ξ ∧ ) ≈ exp ⁡ ( ( ȷ r − 1 Δ ξ + ξ ) ∧ ) . (2.10) \exp(\xi^{\wedge})\exp(\Delta\xi^{\wedge})\approx \exp\left ( (\mathbf{\jmath} ^{-1}_{r}\Delta\xi+\xi )^{\wedge}\right ).\tag{2.10} exp(ξ)exp(Δξ)exp((ȷr1Δξ+ξ)).(2.10)这里 ȷ \jmath ȷ形势比较复杂,它是一个 6 × 6 6\times6 6×6的矩阵,读者可以参考文献[2]中式 ( 7.82 ) (7.82) (7.82) ( 7.83 ) (7.83) (7.83)的内容。
    反之,如果我们在李代数上进行加法,让一个 ξ \xi ξ加上 Δ ξ \Delta \xi Δξ,那么可以近似为李群上带左右雅克比的乘法: exp ⁡ ( ( ξ + Δ ξ ) ∧ ) ≈ exp ⁡ ( ( ȷ l ( ξ ) Δ ξ ) ∧ ) exp ⁡ ( ξ ∧ ) = exp ⁡ ( ξ ∧ ) exp ⁡ ( ( ȷ r ( ξ ) Δ ξ ) ∧ ) . (2.11) \exp\left ( (\xi+\Delta\xi)^{\wedge} \right )\approx\exp\left ( (\boldsymbol{\jmath}_{l}(\xi)\Delta\xi)^{\wedge} \right )\exp(\xi^{\wedge})=\exp(\xi^{\wedge})\exp\left ( (\boldsymbol{\jmath}_{r}(\xi)\Delta\xi)^{\wedge} \right ).\tag{2.11} exp((ξ+Δξ))exp((ȷl(ξ)Δξ))exp(ξ)=exp(ξ)exp((ȷr(ξ)Δξ)).(2.11)
    下面我们正式开始讨论李代数的求导李群扰动对应李代数的扰动模型求导两种思路的异同。

    3. 微分模型——李代数求导

    首先,考虑SO(3)上的情况。假设我们对一个空间点 p p p进行了旋转,得到 R p Rp Rp。现在,计算旋转之后点的坐标相对于旋转的导数,我们非正式地记为: ∂ ( R p ) ∂ R . (3.1) \frac{\partial (Rp)}{\partial R}.\tag{3.1} R(Rp).(3.1)由于 S O ( 3 ) SO(3) SO(3)没有加法,所以该导数无法按照导数的定义进行计算。设 R R R对应的李代数为 ϕ \phi ϕ,我们转而计算3

    ∂ ( exp ⁡ ( ϕ ∧ ) p ) ∂ ϕ \frac{\partial (\exp(\phi^{\wedge })p)}{\partial \phi} ϕ(exp(ϕ)p)按照导数的定义有: ∂ ( exp ⁡ ( ϕ ∧ ) p ) ∂ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( ϕ + δ ϕ ) ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 exp ⁡ ( ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( I + ( J l δ ϕ ) ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p δ ϕ = lim ⁡ δ ϕ → 0 ( J l δ ϕ ) ∧ ( exp ⁡ ( ϕ ∧ ) p ) δ ϕ = lim ⁡ δ ϕ → 0 − ( exp ⁡ ( ϕ ∧ ) p ) ∧ J l δ ϕ δ ϕ = − ( R p ) ∧ J l . \begin{aligned} \frac{\partial (\exp(\phi^{\wedge })p)}{\partial \phi} &= \lim_{\delta \phi\rightarrow 0}\frac{\exp((\phi+\delta\phi)^{\wedge})p-\exp(\phi^{\wedge})p}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{\exp((\boldsymbol{J_{l}}\delta\phi)^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{(\boldsymbol{I}+(\boldsymbol{J_{l}}\delta\phi)^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{(\boldsymbol{J_{l}}\delta\phi)^{\wedge}(\exp(\phi^{\wedge})p)}{\delta\phi} \\&= \lim_{\delta \phi\rightarrow 0}\frac{-(\exp(\phi^{\wedge})p)^{\wedge}\boldsymbol{J_{l}}\delta\phi}{\delta\phi} \\&= -(Rp)^{\wedge}\boldsymbol{J_{l}}. \end{aligned} ϕ(exp(ϕ)p)=δϕ0limδϕexp((ϕ+δϕ))pexp(ϕ)p=δϕ0limδϕexp((Jlδϕ))exp(ϕ)pexp(ϕ)p=δϕ0limδϕ(I+(Jlδϕ))exp(ϕ)pexp(ϕ)p=δϕ0limδϕ(Jlδϕ)(exp(ϕ)p)=δϕ0limδϕ(exp(ϕ)p)Jlδϕ=(Rp)Jl.推导说明:第二行的变换为BCH线性近似,第三行为泰勒展开舍去高阶项后的近似(由于取了极限,可以写等号),第四行至第五行将反对称符号看做叉积,交换之后变号,即 a ∧ b = − b ∧ a a^{\wedge}b=-b^{\wedge}a ab=ba。于是,我们推导出了旋转后的点相对于李代数的导数: ∂ R p ∂ ϕ = ( − R p ) ∧ J l . (3.2) \frac{\partial Rp}{\partial \phi}=(-Rp)^{\wedge}\boldsymbol{J_{l}}.\tag{3.2} ϕRp=(Rp)Jl.(3.2)不过,由于这里仍然含有形势比较复杂的 J l \boldsymbol{J_{l}} Jl,我们不希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算公式。

    4. 扰动模型求导(左乘)

    4.1 SO(3)上的扰动模型求导

    另一种求导方式是对R进行一次扰动 Δ R \Delta R ΔR,然后求结果相对于扰动的变化率。这个扰动可以乘在左边也可以乘在右边,最后结果会有一点儿微小的差异,我们以左扰动为例。设 Δ R \Delta R ΔR对应的李代数为 φ \varphi φ R R R左乘微小旋转 Δ R \Delta R ΔR后就是 Δ R ⋅ R \Delta R\cdot R ΔRR
    这里需要着重强调的是, Δ R ⋅ R \Delta R\cdot R ΔRR非常重要,理解了它就等于理解了扰动,本章的扰动模型求导就是根据 Δ R ⋅ R \Delta R\cdot R ΔRR进行的。
    根据BCH近似,其对应的李代数就是 φ + ϕ \varphi+\phi φ+ϕ,对 φ \varphi φ求导得: ∂ ( R p ) ∂ φ = lim ⁡ φ → 0 ( Δ R ⋅ R ) p − R p φ = lim ⁡ φ → 0 exp ⁡ ( φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 ( I + φ ∧ ) exp ⁡ ( ϕ ∧ ) p − exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 φ ∧ exp ⁡ ( ϕ ∧ ) p φ = lim ⁡ φ → 0 − ( R p ) ∧ φ φ = − ( R p ) ∧ . (3.3) \begin{aligned} \frac{\partial (Rp)}{\partial \varphi} &= \lim_{\varphi\rightarrow 0}\frac{(\Delta R\cdot R)p-Rp}{\varphi} \\&= \lim_{\varphi\rightarrow 0}\frac{\exp(\varphi^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\&= \lim_{\varphi\rightarrow 0}\frac{(\boldsymbol{I}+\varphi^{\wedge})\exp(\phi^{\wedge})p-\exp(\phi^{\wedge})p}{\varphi} \\&= \lim_{\varphi \rightarrow 0}\frac{\varphi^{\wedge}\exp(\phi^{\wedge})p}{\varphi } \\&= \lim_{\varphi \rightarrow 0}\frac{-(Rp)^{\wedge}\varphi }{\varphi } = -(Rp)^{\wedge}. \end{aligned}\tag{3.3} φ(Rp)=φ0limφ(ΔRR)pRp=φ0limφexp(φ)exp(ϕ)pexp(ϕ)p=φ0limφ(I+φ)exp(ϕ)pexp(ϕ)p=φ0limφφexp(ϕ)p=φ0limφ(Rp)φ=(Rp).(3.3)推导说明:第一行是求导公式的直接展开,第二行直接将李群替换为对应的李代数,第三行为泰勒展开舍去高阶小项后的近似(取极限后写等号),第四到第五行即 a ∧ b = − b ∧ a a^{\wedge}b=-b^{\wedge}a ab=ba。可见,相比于直接对李代数求导,省去了一个雅克比 J l \boldsymbol{J_{l}} Jl的计算。这使得扰动模型更为实用。请读者务必理解这里的求导运算,这在位姿估计中具有重要的意义。

    4.2 SE(3)上的扰动模型求导

    下边,我们给出 S E ( 3 ) SE(3) SE(3)上的扰动模型。假设某空间点p经过一次变换 T \boldsymbol{T} T(对应李代数为 ξ \xi ξ),得到 T p \boldsymbol{T}p Tp4。现在给 T \boldsymbol{T} T左乘一个扰动 Δ T = exp ⁡ ( χ ∧ ) \Delta T=\exp(\chi^{\wedge}) ΔT=exp(χ),我们设扰动项的李代数为 χ = [ Δ ρ , Δ ϕ ] T \chi=[\Delta \rho,\Delta\phi]^{T} χ=[Δρ,Δϕ]T,那么:

    ∂ T p ∂ χ = lim ⁡ χ → 0 ( Δ T ⋅ T ) p − T p χ = lim ⁡ χ → 0 exp ⁡ ( χ ∧ ) exp ⁡ ( ξ ∧ ) p − exp ⁡ ( ξ ∧ ) p χ = lim ⁡ χ → 0 ( I + χ ∧ ) exp ⁡ ( ξ ∧ ) p − exp ⁡ ( ξ ∧ ) p χ = lim ⁡ χ → 0 χ ∧ exp ⁡ ( ξ ∧ ) p χ = lim ⁡ χ → 0 [ Δ ϕ ∧ Δ ρ 0 T 0 ] [ R p + t 1 ] χ = lim ⁡ χ → 0 [ Δ ϕ ∧ ( R p + t ) + Δ p 0 T ] [ Δ ρ , Δ ϕ ] T = [ I − ( R p + t ) ∧ 0 T 0 T ] = d e f ( T p ) ⊙ . (3.4) \begin{aligned} \frac{\partial \boldsymbol{T}p}{\partial \chi} &= \lim_{\chi\rightarrow 0} \frac{(\Delta T\cdot T)p-Tp}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{\exp(\chi^{\wedge})\exp(\xi^{\wedge})p-\exp(\xi^{\wedge})p}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{(I+\chi^{\wedge})\exp(\xi^{\wedge})p-\exp(\xi^{\wedge})p}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{\chi^{\wedge}\exp(\xi^{\wedge})p}{\chi}\\&= \lim_{\chi\rightarrow 0} \frac{\begin{bmatrix} \Delta\phi^{\wedge} & \Delta \rho\\ \boldsymbol{0}^{T} & 0 \end{bmatrix}\begin{bmatrix} Rp+t\\ 1 \end{bmatrix}}{\chi} \\&= \lim_{\chi\rightarrow 0} \frac{\begin{bmatrix} \Delta\phi^{\wedge}(Rp+t)+\Delta p\\ \boldsymbol{0}^{T} \end{bmatrix}}{[\Delta \rho,\Delta\phi]^{T}} \\&= \begin{bmatrix} \boldsymbol{I} & -(Rp+t)^{\wedge}\\ \boldsymbol{0}^{T} & 0^{T} \end{bmatrix} \overset{\underset{\mathrm{def}}{}}{=}(Tp)^{\odot }. \end{aligned}\tag{3.4} χTp=χ0limχ(ΔTT)pTp=χ0limχexp(χ)exp(ξ)pexp(ξ)p=χ0limχ(I+χ)exp(ξ)pexp(ξ)p=χ0limχχexp(ξ)p=χ0limχ[Δϕ0TΔρ0][Rp+t1]=χ0lim[Δρ,Δϕ]T[Δϕ(Rp+t)+Δp0T]=[I0T(Rp+t)0T]=def(Tp).(3.4)我们把最后的结果定义成一个算符 ⊙ \odot 5,它把一个齐次坐标的空间点变换成一个 4 × 6 4\times 6 4×6的矩阵。
    上式推导中,需要稍微解释的是第五行的矩阵求导顺序,假设 a , b , x , y \boldsymbol{a,b,x,y} a,b,x,y都是列向量,那么在我们的符号写法下,有如下规则: d [ a b ] d [ x y ] = ( d [ a , b ] T d [ x y ] ) T = [ d a d x d b d x d a d y d b d y ] T = [ d a d x d a d y d b d x d b d y . ] (3.5) \frac{d\begin{bmatrix} a\\ b \end{bmatrix}}{d\begin{bmatrix} x\\ y \end{bmatrix}}=\left ( \frac{d[a,b]^{T}}{d\begin{bmatrix} x\\ y \end{bmatrix}} \right )^{T}=\begin{bmatrix} \frac{da}{dx} & \frac{db}{dx}\\ \frac{da}{dy} & \frac{db}{dy} \end{bmatrix}^{T}=\begin{bmatrix} \frac{da}{dx} & \frac{da}{dy}\\ \frac{db}{dx} & \frac{db}{dy}. \end{bmatrix}\tag{3.5} d[xy]d[ab]=d[xy]d[a,b]TT=[dxdadydadxdbdydb]T=[dxdadxdbdydadydb.](3.5)

    至此,我们已经介绍了李群与李代数上的微分运算。下面再补充一点其它性质。

    4.3 伴随性质

    对于公式: R p ∧ R T = ( R p ) ∧ . (3.6) Rp^{\wedge}R^{T}=(Rp)^{\wedge}.\tag{3.6} RpRT=(Rp).(3.6)有类似的公式: R exp ⁡ ( p ∧ ) R T = exp ⁡ ( ( R p ) ∧ ) . (3.7) R\exp(p^{\wedge})R^{T}=\exp((Rp)^{\wedge}).\tag{3.7} Rexp(p)RT=exp((Rp)).(3.7)该式称为 S O ( 3 ) SO(3) SO(3)上的伴随性质。同样地,在 S E ( 3 ) SE(3) SE(3)上也有伴随性质: T exp ⁡ ( ξ ∧ ) T − 1 = exp ⁡ ( ( A d ( T ) ξ ) ∧ ) , (3.8) T\exp(\xi^{\wedge})T^{-1}=\exp((Ad(T)\xi)^{\wedge}),\tag{3.8} Texp(ξ)T1=exp((Ad(T)ξ)),(3.8)其中: A d ( T ) = [ R t ∧ R 0 R ] . Ad(T)=\begin{bmatrix} R & t^{\wedge}R \\ 0 & R \end{bmatrix}. Ad(T)=[R0tRR].本性质是书中习题,感兴趣的同学可以研究下。

    5. 相似变换群相关

    最后,我们要提一下在单目视觉中使用的相似变换群 S i m ( 3 ) Sim(3) Sim(3),它对应的李代数,以及他们之间的指数/对数映射关系,最后看一下它对应的扰动模型。如果你只对双目SLAM或RGB-D SLAM感兴趣,可以跳过本节。因仿射变换和射影变换不再具备指数/对数映射性质和扰动模型,所以不再考虑它们。

    5.1 相似变换群 S i m ( 3 ) Sim(3) Sim(3)

    我们已经介绍过单目的尺度不确定性。如果在单目SLAM中使用 S E ( 3 ) SE(3) SE(3)表示位姿,那么由于尺度不确定性与尺度漂移,整个SLAM过程中的尺度会发生变化,这在 S E ( 3 ) SE(3) SE(3)中未能体现出来。因此,在单目情况下,我们一般会显示地把尺度因子表达出来。用数学语言来说,对于位于空间的点 p p p,在相机坐标系下要经过一个相似变换,而非欧氏变换: p ′ = [ s R t 0 T 1 ] p = s R p + t . (5.1) p'=\begin{bmatrix} s\boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0}^{T} & 1 \end{bmatrix}p=s\boldsymbol{R}\boldsymbol{p}+\boldsymbol{t}.\tag{5.1} p=[sR0Tt1]p=sRp+t.(5.1)在相似变换中,我们把尺度 s s s表达出来了,它同时作用在 p p p的3个坐标之上,对 p p p进行了一次缩放。
    S O ( 3 ) 、 S E ( 3 ) SO(3)、SE(3) SO(3)SE(3)类似,相似变换也对矩阵乘法构成群,称为相似变换群 S i m ( 3 ) Sim(3) Sim(3) S i m ( 3 ) = { S = [ s R t 0 T 1 ] ∈ R 4 × 4 } . (5.2) Sim(3)=\left \{ \boldsymbol{S}=\begin{bmatrix} s\boldsymbol{R} & \boldsymbol{t}\\ \boldsymbol{0}^{T} & 1 \end{bmatrix} \in \mathbb{R}^{4\times 4}\right \}.\tag{5.2} Sim(3)={S=[sR0Tt1]R4×4}.(5.2)同样地, S i m ( 3 ) Sim(3) Sim(3)也有对应的李代数、指数映射、对数映射、扰动模型等,下边分别介绍。

    5.2 李代数 s i m ( 3 ) \mathfrak{sim}(3) sim(3)及指数/对数映射

    5.2.1 李代数 s i m ( 3 ) \mathfrak{sim}(3) sim(3)

    李代数 s i m ( 3 ) \mathfrak{sim}(3) sim(3)是一个7维向量 ζ \zeta ζ,它的前6维与 s e ( 3 ) \mathfrak{se}(3) se(3)相同,最后多了一项相似变换系数 σ \sigma σ s i m ( 3 ) = { ζ = [ ρ ϕ σ ] ∈ R 7 , ρ ∈ R 3 , ϕ ∈ s o ( 3 ) , ζ ∧ = [ σ I + ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } . (5.3) \mathfrak{sim}(3)=\left \{ \boldsymbol{\zeta}=\begin{bmatrix} \boldsymbol{\rho} \\ \boldsymbol{\phi} \\ \sigma \end{bmatrix}\in \mathbb{R}^{7},\boldsymbol{\rho}\in\mathbb{R}^{3},\boldsymbol{\phi} \in \mathfrak{so}(3),\boldsymbol{\zeta}^{\wedge}=\begin{bmatrix} \sigma\boldsymbol{I}+\boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho}\\ \boldsymbol{0}^{T} & 0 \end{bmatrix}\in \mathbb{R}^{4\times4} \right \}.\tag{5.3} sim(3)=ζ=ρϕσR7,ρR3,ϕso(3),ζ=[σI+ϕ0Tρ0]R4×4.(5.3)

    5.2.2 李代数到李群的指数映射

    关联 S i m ( 3 ) Sim(3) Sim(3) s i m ( 3 ) \mathfrak{sim}(3) sim(3)的仍是指数映射和对数映射。先看李代数 s i m ( 3 ) \mathfrak{sim}(3) sim(3)到李群 S i m ( 3 ) Sim(3) Sim(3)的指数映射: exp ⁡ ( ζ ∧ ) = [ e σ exp ⁡ ( ϕ ∧ ) J s ρ 0 T 1 ] . (5.4) \exp(\boldsymbol{\zeta}^{\wedge})=\begin{bmatrix} e^{\sigma}\exp(\boldsymbol{\phi}^{\wedge}) & \boldsymbol{J}_{s}\boldsymbol{\rho}\\ \boldsymbol{0}^{T} & 1 \end{bmatrix}.\tag{5.4} exp(ζ)=[eσexp(ϕ)0TJsρ1].(5.4)其中: J s = e σ − 1 σ I + σ e σ sin ⁡ θ + ( 1 − e σ cos ⁡ θ ) θ σ 2 + θ 2 a ∧ + ( e σ − 1 σ − ( e σ cos ⁡ θ − 1 ) σ + ( e σ sin ⁡ θ ) θ σ 2 + θ 2 ) a ∧ a ∧ \begin{aligned} \boldsymbol{J}_{s} &= \frac{e^{\sigma}-1}{\sigma}\boldsymbol{I}+\frac{\sigma e^{\sigma}\sin\theta+(1-e^{\sigma}\cos\theta)\theta}{\sigma^{2}+\theta^{2}}\boldsymbol{a}^{\wedge}+ \\& \quad\quad \left (\frac{e^{\sigma}-1}{\sigma}-\frac{(e^{\sigma}\cos\theta-1)\sigma+(e^{\sigma}\sin\theta)\theta}{\sigma^{2}+\theta^{2}} \right )\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge} \end{aligned} Js=σeσ1I+σ2+θ2σeσsinθ+(1eσcosθ)θa+(σeσ1σ2+θ2(eσcosθ1)σ+(eσsinθ)θ)aa通过上式可知,已知李代数 ζ \zeta ζ,它与李群中元素的对应关系为: s = e σ , R = exp ⁡ ( ϕ ∧ ) , t = J s ρ . (5.5) s=e^{\sigma},\boldsymbol{R}=\exp(\boldsymbol{\phi}^{\wedge}),\boldsymbol{t}=\boldsymbol{J}_{s}\boldsymbol{\rho}.\tag{5.5} s=eσ,R=exp(ϕ),t=Jsρ.(5.5)说明:李群中的尺度因子 s s s即为李代数中 σ \sigma σ的指数函数;旋转部分与 S O ( 3 ) SO(3) SO(3)中一致;平移部分,在 s e ( 3 ) \mathfrak{se}(3) se(3)中需要乘一个雅克比 J \boldsymbol{J} J,而相似变换中是更为复杂的 J s \boldsymbol{J}_{s} Js

    5.2.3 李群到李代数的对数映射

    已知李群 S i m ( 3 ) Sim(3) Sim(3)时,先将矩阵简化为如下形式:左下角最后一行前6个元素为0,最后一个元素为1。求解各元素的步骤叙述如下:

    1. σ \sigma σ。对于矩阵左上角 3 × 3 3\times 3 3×3的行列式,由于 ∥ R ∥ = 1 \left \| \boldsymbol{R}\right \|=1 R=1,故行列式的值就是尺度因子 s s s,此时 σ = l n ( s ) \sigma=ln(s) σ=ln(s)
    2. ϕ \boldsymbol{\phi} ϕ。 简化后的李群 S i m ( 3 ) Sim(3) Sim(3)的左上角 3 × 3 3\times 3 3×3矩阵除以尺度因子 s s s,得到旋转矩阵 R \boldsymbol{R} R,根据 R \boldsymbol{R} R和罗德里格斯公式可求得 a \boldsymbol{a} a θ \theta θ,进而由 ϕ = a θ \boldsymbol{\phi}=\boldsymbol{a}\theta ϕ=aθ求得 ϕ \boldsymbol{\phi} ϕ
    3. ρ \boldsymbol{\rho} ρ。由 σ 、 a 、 θ \sigma、\boldsymbol{a}、\theta σaθ可求得 J s \boldsymbol{J}_{s} Js,简化后的李群 S i m ( 3 ) Sim(3) Sim(3)右上角一列为 t \boldsymbol{t} t,此时 ρ = t / J s \boldsymbol{\rho}=\boldsymbol{t}/\boldsymbol{J}_{s} ρ=t/Js

    最后由 σ \sigma σ ϕ \boldsymbol{\phi} ϕ ρ \boldsymbol{\rho} ρ组合,得到对应的李代数 ζ \boldsymbol{\zeta} ζ

    5.3 扰动模型

    S i m ( 3 ) Sim(3) Sim(3)的BCH近似与 S E ( 3 ) SE(3) SE(3)是类似的,我们可以讨论一个点 p p p经过相似变换 S p Sp Sp后,相对于 S S S的导数。同样地,存在微分模型和扰动模型两种方式,而扰动模型不用计算雅克比,较为简单。因此我们省略推导过程,直接给出扰动模型的结果。
    设给予 S p Sp Sp左侧一个小扰动 exp ⁡ ( η ∧ ) \exp(\eta ^{\wedge}) exp(η),并求 S p Sp Sp对于扰动的导数。因为 S p Sp Sp是4维的齐次坐标, η \eta η是7维向量,所以该导数应该是 4 × 7 4\times7 4×7的雅克比。方便起见,记 S p Sp Sp的前3维组成向量为 q q q,那么: ∂ S p ∂ η = [ I − q ∧ q 0 T 0 T 0 ] . (5.6) \frac{\partial \boldsymbol{Sp}}{\partial \boldsymbol{\eta}}=\begin{bmatrix} \boldsymbol{I} & -\boldsymbol{q}^{\wedge} & \boldsymbol{q}\\ \boldsymbol{0}^{T} & \boldsymbol{0}^{T} & 0 \end{bmatrix}.\tag{5.6} ηSp=[I0Tq0Tq0].(5.6)此式可对比公式 ( 3.4 ) (3.4) (3.4)理解记忆。关于 S i m ( 3 ) Sim(3) Sim(3),我们就介绍到这里。关于其更详细的资料,建议读者参见文献[3]。

    5.4 总结

    本讲《李群与李代数》引入了李群 S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3),以及它们对应的李代数 s o ( 3 ) \mathfrak{so}(3) so(3) s e ( 3 ) \mathfrak{se}(3) se(3)。我们介绍了位姿在他们上面的表达和转换,然后通过BCH的线性近似,就可以对位子进行扰动并求导。这给之后讲解位姿的优化打下了理论基础,因为我们需要经常对某一个位姿的估计值进行调整,使它对应的误差减小,因此只有弄清楚如何对位姿进行调整和更新之后,才能继续下一步的内容。

    本讲内容比较偏理论,讲的过程也非常精简,速度也相对快,但本讲内容是解决后续许多问题的基础,请读者务必理解,特别是位姿估计部分。值得一提的是,除了李代数,同样也可以用四元数、欧拉角等方式表示旋转,只是后续处理要麻烦一些。在实际应用中,也可以使用 S O ( 3 ) SO(3) SO(3)加上平移的方式来代替 S E ( 3 ) SE(3) SE(3),从而回避一些雅克比的计算。

    为方便理解,下面来看两个具体的例子。第一个例子是李群和李代数的基本操作,第二个例子是关于最小误差的计算。

    6. 编码实战

    6.1 李群与李代数的运算库:Sophus

    我们已经介绍了李代数的入门知识,现在是通过实践演练并巩固所学知识的时候了。第3讲中的Eigen提供了几何模块,但没有提供李代数的支持。一个较好的李代数库Strasdat维护的Sophu库6,它支持本章讨论的 S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3),此外,还有二维运动 S O ( 2 ) SO(2) SO(2) S E ( 2 ) SE(2) SE(2)及相似变换 S i m ( 3 ) Sim(3) Sim(3)的内容。它是直接在Eigen基础上开发的,因此我们不需要安装额外的依赖库。读者可以直接从GitHub上获取Sophus,原书目录slambook/3rdparty也提供Sophus源代码。

    由于历史原因,Sophus早期版本只提供双精度的李群/李代数类,后续版本改写成了模板类。模板类的Sophus可以使用不同精度的李群/李代数,但同时也增加了使用难度。本书中使用带模板类的Sophus库,Sophus本身也是一个cmake工程,它只需编译即可,无需安装。

    下面来演示Sophus库中的 S O ( 3 ) SO(3) SO(3) S E ( 3 ) SE(3) SE(3)运算:

    #include<iostream>
    #include<cmath>
    #include<eigen3/Eigen/Core>
    #include<eigen3/Eigen/Geometry>
    #include "sophus/se3.hpp"
    
    using namespace std;
    using namespace Eigen;
    using namespace Sophus;
    
    int main(int argc, char **argv) {
        Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();
        Quaterniond q(R);
        Sophus::SO3d SO3_R(R);
        Sophus::SO3d SO3_q(q);
        cout<<"SO(3) from matrix:\n"<<SO3_R.matrix()<<endl;
        cout<<"SO(3) from quaternion:\n"<<SO3_q.matrix()<<endl;
        cout<<"they are equal"<<endl;
        
        Vector3d so3 = SO3_R.log();
        cout<<"so3 = "<<so3.transpose()<<endl;
        cout<<"so3 hat = \n"<<Sophus::SO3d::hat(so3)<<endl;
        cout<<"so3 hat vee = "<<Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose()<<endl;
        
        Vector3d update_so3(1e-4, 0, 0);
        Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_R;
        cout<<"SO3 updated = \n" <<endl<<SO3_updated.matrix()<<endl;
        
        cout<<"******************************"<<endl;
        
        Vector3d t(1, 0, 0);
        Sophus::SE3d SE3_Rt(R, t);
        Sophus::SE3d SE3_qt(q, t);
        cout<<"SE(3) from R, t:\n"<<SE3_Rt.matrix()<<endl;
        cout<<"SE(3) from q, t:\n"<<SE3_qt.matrix()<<endl;
        cout<<"they are equal"<<endl;
        
        typedef Eigen::Matrix<double, 6, 1> Vector6d;
        Vector6d se3 = SE3_Rt.log();
        cout<<"se3 = "<<se3.transpose()<<endl;
        cout<<"se3 hat = \n"<<Sophus::SE3d::hat(se3)<<endl;
        cout<<"se3 hat vee = "<<Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose()<<endl;
        
        Vector6d update_se3;
        update_se3.setZero();
        update_se3(0, 0) = 1e-4;
        cout<<"update_se3 = \n" <<endl<<update_se3.matrix()<<endl;
        Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
        cout<<"SE3 updated = \n" <<endl<<SE3_updated.matrix()<<endl;
        
        return 0;
    }
    
    

    该演示程序分为两部分:前半部分介绍 S O ( 3 ) SO(3) SO(3)上的操作,后半部分为 S E ( 3 ) SE(3) SE(3)。我们演示如何构造 S O ( 3 ) , S E ( 3 ) SO(3),SE(3) SO(3),SE(3)对象,对它们进行指数、对数映射,以及当知道更新量后,如何对李群元素进行更新。为了编译它,请在CMakeLists.txt添加以下几行代码:

    cmake_minimum_required(VERSION 3.0)
    
    project(useSophus)
    
    # 为使用 sophus,需要使用find_package命令找到它
    find_package(Sophus REQUIRED)
    find_package(Pangolin REQUIRED)
    include_directories(${Sophus_INCLUDE_DIRS})
    include_directories(${Pangolin_INCLUDE_DIRS})
    
    add_executable(trajectoryError trajectoryError.cpp)
    target_link_libraries(trajectoryError ${Pangolin_LIBRARIES})
    
    include_directories("/usr/include/eigen3")
    include_directories("/home/frank/slam14/slambook2/3rdparty/Sophus")
    
    add_executable(useSophus useSophus.cpp)
    
    # 把工程调为Debug编译模式
    set(CMAKE_BUILD_TYPE "Debug")
    
    

    find_package命令是cmake提供的寻找某个库的头文件和库文件的指令。如果cmake能找到它,就会提供头文件和库文件所在目录的变量,在这个例子中,就是Sophus_INCLUDE_DIRS。基于模板的Sophu库和Eigen库一样,是仅含头文件而没有源文件的,根据它们,我们就能将Sophus库引入自己的cmake工程。程序运行结果如下:

    SO(3) from matrix:
    2.22045e-16          -1           0
              1 2.22045e-16           0
              0           0           1
    SO(3) from quaternion:
    2.22045e-16          -1           0
              1 2.22045e-16           0
              0           0           1
    they are equal
    so3 =      0      0 1.5708
    so3 hat = 
          0 -1.5708       0
     1.5708       0      -0
         -0       0       0
    so3 hat vee =      0      0 1.5708
    SO3 updated = 
    
              0          -1           0
              1           0     -0.0001
         0.0001 2.03288e-20           1
    ******************************
    SE(3) from R, t:
    2.22045e-16          -1           0           1
              1 2.22045e-16           0           0
              0           0           1           0
              0           0           0           1
    SE(3) from q, t:
    2.22045e-16          -1           0           1
              1 2.22045e-16           0           0
              0           0           1           0
              0           0           0           1
    they are equal
    se3 =  0.785398 -0.785398         0         0         0    1.5708
    se3 hat = 
            0   -1.5708         0  0.785398
       1.5708         0        -0 -0.785398
           -0         0         0         0
            0         0         0         0
    se3 hat vee =  0.785398 -0.785398         0         0         0    1.5708
    update_se3 = 
    
    0.0001
         0
         0
         0
         0
         0
    SE3 updated = 
    
    2.22045e-16          -1           0      1.0001
              1 2.22045e-16           0           0
              0           0           1           0
              0           0           0           1
    
    

    6.2 评估轨迹的误差

    在实际工程中,我们经常需要评估一个算法的估计轨迹与真实轨迹的差异来评价算法的精度。真实轨迹往往通过某些更高精度的系统获得,而估计轨迹则是由待评价的算法计算得到的。在三维刚体运动中,我们演示了如何显示存储在文件中的某条轨迹,本节我们考虑如何计算两条轨迹的误差。

    考虑一条估计轨迹 T e s t i , i \boldsymbol{T}_{esti,i} Testi,i和真实轨迹(ground truth) T g t , i \boldsymbol{T}_{gt,i} Tgt,i,其中 i = 1 , ⋯   , N i=1,\cdots,N i=1,,N,那么我们可以定义一些误差指标来描述它们之间的差别。常见的误差指标有很多种,介绍如下:

    1. 绝对轨迹误差(Absolute Trajectory Error, ATE),计算公式如下: A T E a l l = 1 N ∑ i = 1 N ∥ log ⁡ ( T g t , i − 1 T e s t i , i ) ∨ ∥ 2 . (6.1) ATE_{all}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| \log(\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{esti,i})^{\vee} \right \|^{2}}.\tag{6.1} ATEall=N1i=1Nlog(Tgt,i1Testi,i)2 .(6.1)这实际上是每个位姿李代数的均方根误差(Root-Mean-Squared Error,RMSE),它可以刻画两条轨迹的旋转和平移误差。
    2. 平均平移误差(Average Translational Error),也有的文献仅考虑平移误差,计算公式如下: A T E t r a n s = 1 N ∑ i = 1 N ∥ t r a n s ( T g t , i − 1 T e s t i , i ) ∥ 2 . (6.2) ATE_{trans}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| trans(\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{esti,i}) \right \|^{2}}.\tag{6.2} ATEtrans=N1i=1Ntrans(Tgt,i1Testi,i)2 .(6.2)其中trans表示取括号内部变量的平移部分。因为从整条轨迹来看,旋转出现误差后,随后也会在平移上出现误差,所以以上两种指标在实际中都适用。
    3. 相对位姿误差(Relative Pose Error,RPE),除此之外,还可以定义相对误差。例如,考虑 i i i时刻到 i + Δ t i+\Delta t i+Δt时刻的运动,那么RPE可定义为: R P E a l l = 1 N ∑ i = 1 N ∥ log ⁡ ( ( T g t , i − 1 T g t , i + Δ t ) − 1 ( T e s t i , i − 1 T e s t i , i + Δ t ) ) ∨ ∥ 2 . (6.3) RPE_{all}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| \log\left ((\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{gt,i+\Delta t})^{-1}(\boldsymbol{T}^{-1}_{esti,i}\boldsymbol{T}_{esti,i+\Delta t} )\right )^{\vee} \right \|^{2}}.\tag{6.3} RPEall=N1i=1Nlog((Tgt,i1Tgt,i+Δt)1(Testi,i1Testi,i+Δt))2 .(6.3)同样地,也可只取平移部分: R P E t r a n s = 1 N ∑ i = 1 N ∥ t r a n s ( ( T g t , i − 1 T g t , i + Δ t ) − 1 ( T e s t i , i − 1 T e s t i , i + Δ t ) ) ∨ ∥ 2 RPE_{trans}=\sqrt{\frac{1}{N}\sum_{i=1}^{N}\left \| trans \left ((\boldsymbol{T}^{-1}_{gt,i}\boldsymbol{T}_{gt,i+\Delta t})^{-1}(\boldsymbol{T}^{-1}_{esti,i}\boldsymbol{T}_{esti,i+\Delta t} )\right )^{\vee} \right \|^{2}} RPEtrans=N1i=1Ntrans((Tgt,i1Tgt,i+Δt)1(Testi,i1Testi,i+Δt))2 利用Sophus库,很容易实现这部分计算。下面我们演示绝对轨迹误差的计算。在这个例子中,我们有groundtruth.txt和estimated.txt两条轨迹,下面的代码江都区这两条轨迹,计算RMSE然后显示到3D窗口中:
    #include<iostream>
    #include<fstream>
    #include<unistd.h>
    #include<pangolin/pangolin.h>
    #include<sophus/se3.hpp>
    
    using namespace Sophus;
    using namespace std;
    
    string groundtruth_file = "../groundtruth.txt";
    string estimated_file = "../estimated.txt";
    
    typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
    
    void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);
    
    TrajectoryType ReadTrajectory(const string &path);
    
    int main(int argc, char **argv) {
        TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
        TrajectoryType estimated = ReadTrajectory(estimated_file);
        cout<<groundtruth_file<<endl;
        assert(!groundtruth.empty() && !estimated.empty());
        assert(groundtruth.size() == estimated.size());
        
        double rmse = 0;
        for (size_t i=0; i<estimated.size(); i++) {
            Sophus::SE3d p1 = estimated[1], p2 = groundtruth[i];
            double error = (p2.inverse() * p1).log().norm();
            rmse += error * error;
        }
        rmse = rmse/double(estimated.size());
        rmse = sqrt(rmse);
        cout<<"RMSE = "<<rmse<<endl;
        
        DrawTrajectory(groundtruth, estimated);
        return 0;
    }
    
    TrajectoryType ReadTrajectory(const string& path) {
        ifstream fin(path);
        TrajectoryType trajectory;
        if (!fin) {
            cerr<<"trajectory: "<<path<<" not found."<<endl;
            return trajectory;
        }
        
        while (!fin.eof()) {
            double time, tx, ty, tz, qx, qy, qz, qw;
            fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
            Sophus::SE3d p1(Eigen::Quaterniond(qx, qy, qz, qw), Eigen::Vector3d(tx, ty, tz));
            trajectory.push_back(p1);
        }
        
        return trajectory;
    }
    
    void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
      // create pangolin window and plot the trajectory
      pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
      glEnable(GL_DEPTH_TEST);
      glEnable(GL_BLEND);
      glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
    
      pangolin::OpenGlRenderState s_cam(
          pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
          pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
      );
    
      pangolin::View &d_cam = pangolin::CreateDisplay()
          .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
          .SetHandler(new pangolin::Handler3D(s_cam));
    
    
      while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    
        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    
        glLineWidth(2);
        for (size_t i = 0; i < gt.size() - 1; i++) {
          glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
          glBegin(GL_LINES);
          auto p1 = gt[i], p2 = gt[i + 1];
          glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
          glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
          glEnd();
        }
    
        for (size_t i = 0; i < esti.size() - 1; i++) {
          glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
          glBegin(GL_LINES);
          auto p1 = esti[i], p2 = esti[i + 1];
          glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
          glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
          glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
      }
    
    }
    
    

    代码中文本文件groundtruth.txt和estimated.txt请从博客资源部分下载。
    该程序输出结果为RMSE = 3.09706。读者也可以尝试将旋转部分去掉,仅计算平移部分的误差。这个例子中的数据已经经过预处理,包括轨迹的时间对齐、外参预估等,请读者知悉。程序运行后的绘制图如下:在这里插入图片描述

    后记:

    SLAM系列到此完结。笔者认为数学部分难以理解的部分已经补充完毕,剩下的部分,高翔老师的书讲解的没那么混乱了,仔细阅读可以理解。后边十讲的部分,笔者会以思维导图的形式继续发博客总结,喜欢的童鞋记得关注我吧。

    参考文献:

    1. 《视觉SLAM十四讲:从理论到实践》,高翔、张涛等著,中国工信出版社。
    2. T.Barfoot, State estimation for robotics: A matrix lie group approach, 2016.
    3. H. Strasdat, Local accuracy and global consistency for efficient visual slam. PhD thesis, Citeseer, 2012.

    1. 参见https://groupprops.subwiki.org/wiki/Baker-Campbell-Hausdorff_formula。 ↩︎

    2. 对BCH具体形式和近似表达的具体推导,本文不作具体讨论,请参考文献[2]。 ↩︎

    3. 严格来说,在矩阵微分中,只能求行向量关于列向量的导数,所得结果是一个矩阵。但本文写成列向量对列向量的导数,读者可以认为先对分子进行转置,再对最后结果进行转置,这使得式子变得简洁。在这种意义下,可以认为 d ( A x ) d x = A \frac{d(Ax)}{dx}=A dxd(Ax)=A ↩︎

    4. 请注意,为了使乘法成立, p p p必须使用齐次坐标。 ↩︎

    5. 作者将其读作“咚”,像一个石子掉在井里的声音。 ↩︎

    6. 最早提出李代数的是Sophus Lie,因此库以他的名字命名。 ↩︎

    展开全文
  • 凸优化简单学习笔记

    2019-01-24 23:21:04
    本文来源于个人的凸优化学习笔记参考cs229 cvxoptnote,写成笔记的原因仅仅是想通过个人的笔记自己讲述与推导一下这些...凸集的交, 设 Ci,i=1,2,3,...,nC_i,i = 1,2,3,...,nCi​,i=1,2,3,...,n 是凸集,那么我们有...

    本文来源于个人的凸优化学习笔记参考cs229 cvxoptnote,写成笔记的原因仅仅是想通过个人的笔记自己讲述与推导一下这些数学公式,内容可能会很简单,强力建议想得到一手资料的人好好学习文末参考资料

    凸集合

    定义就直接跳过了,这里简单写一些常见的凸集

    • 凸集的交, 设 C i , i = 1 , 2 , 3 , . . . , n C_i,i = 1,2,3,...,n Ci,i=1,2,3,...,n 是凸集,那么我们有 ∩ i = 0 n C i \cap _{i=0}^n C_i i=0nCi 也是凸集
    • affine subspace A x + b = 0 , x ∈ R n Ax+b=0,x \in R^n Ax+b=0,xRn, 这既是 convex,也是concave
    • 凸函数的sublevel i f   f ( x )   i s   c o n v e x   f u n c t i o n , t h e n { f ( x ) &lt; = α } i s   c o n v e x   s e t if \ f(x)\ is\ convex\ function,then \{f(x) &lt;=\alpha\} is\ convex\ set if f(x) is convex function,then{f(x)<=α}is convex set

    凸函数

    Define

    convex

    性质,两条重点

    1. first-order-approximation

      f ( y ) ≥ f ( x ) + ( ∇ f ( x ) ) T ( y − x ) f(y)\ge f(x)+(\nabla f(x))^T(y-x) f(y)f(x)+(f(x))T(yx)
    2. 二阶导数大于0

      ∇ x 2 f ( x ) ≥ 0 \nabla_x^2 f(x) \ge 0 x2f(x)0

    一些简单的凸函数

    1. Affine funciton

      f ( x ) = A T x + b , x ∈ R n f(x) =A^Tx+b,x\in R^n f(x)=ATx+b,xRn
    2. quadratic function

      f : R n − &gt; R , f ( x ) = 2 − 1 x T A x + b T x + c , A ∈ S n , b ∈ R n , c ∈ R f:R^n-&gt;R,f(x) = 2^{-1}x^TAx+b^Tx+c,A\in S^n,b\in R^n,c\in R f:Rn>R,f(x)=21xTAx+bTx+c,ASn,bRn,cR

      这里简单说一下 ∇ 2 f ( x ) = A \nabla^2 f(x)=A 2f(x)=A, 因此如果 A A A 是半正定(positive-semidefinite)的,那么 f ( x ) f(x) f(x) is convex
    3. Norm,e.g f ( x ) = ∣ ∣ x ∣ ∣ 2 2 f(x) = ||x||_2^2 f(x)=x22
    4. 凸函数的非负权重和

      f ( x ) = ∑ i = 0 n c i f i ( x ) , f i ( x ) ∈ c o n v e x   f u n c t i o n   c l a s s f(x)=\sum_{i=0}^nc_if_i(x),f_i(x)\in convex\ function\ class f(x)=i=0ncifi(x),fi(x)convex function class
    5. 凸函数集合的最大值函数

      f ( x ) = max ⁡ f i ( x ) , f i ( x ) ∈ c o n v e x   f u n c t i o n   c l a s s f(x) = \max f_i(x),f_i(x)\in convex\ function\ class f(x)=maxfi(x),fi(x)convex function class

      证明:
      f ( θ x + ( 1 − θ ) y ) = max ⁡ f i ( θ x + ( 1 − θ ) y ) ≤ max ⁡ θ f i ( x ) + ( 1 − θ ) f i ( y ) ≤ θ f ( x ) + ( 1 − θ ) f ( y ) \begin{aligned} f(\theta x + (1-\theta)y)&amp;= \max f_i(\theta x + (1-\theta)y)\\ &amp;\le \max \theta f_i(x) +(1-\theta)f_i(y)\\ &amp;\le \theta f(x) + (1-\theta)f(y) \end{aligned} f(θx+(1θ)y)=maxfi(θx+(1θ)y)maxθfi(x)+(1θ)fi(y)θf(x)+(1θ)f(y)

    凸优化问题

    define

    凸优化问题的局部最优解一定是全局最优解

    接下来是凸优化问题最重要,也是最有趣的一个性质

    证明

    我们用反证法,
    x ∈ R n , f ( x ) ∈ c o n v e x   f u n c t i o n   c l a s s x\in R^n,f(x) \in convex\ function\ class xRn,f(x)convex function class,且, x x x 是 凸优化问题 f ( x ) f(x) f(x) 的一个局部最优解,那么我们有, x x x 的邻域中,i.e. ∣ ∣ x − z ∣ ∣ 2 &lt; R ||x-z||_2 &lt;R xz2<R ,不存在一个点 z z z,使得 f ( z ) &lt; f ( x ) f(z)&lt;f(x) f(z)<f(x), 同时,假设 x x x 不是全局最优解,那么我们有,在定义域中,存在一点 y , s . t .   f ( y ) &lt; f ( x ) y,s.t.\ f(y)&lt;f(x) y,s.t. f(y)<f(x)

    z = θ y + ( 1 − θ ) x , θ = R 2 ∣ ∣ x − y ∣ ∣ 2 z=\theta y + (1-\theta)x,\theta=\frac{R}{2||x-y||_2} z=θy+(1θ)x,θ=2xy2R,可以证明 ∣ ∣ x − z ∣ ∣ 2 = 2 − 1 R &lt; R ||x-z||_2=2^{-1}R&lt;R xz2=21R<R

    同时,

    f ( z ) = f ( θ y + ( 1 − θ ) x ) ≤ θ f ( y ) + ( 1 − θ ) f ( x ) ≤ f ( x ) \begin{aligned} f(z)&amp;=f(\theta y + (1-\theta)x)\\ &amp;\le \theta f(y)+(1-\theta)f(x)\\ &amp;\le f(x) \end{aligned} f(z)=f(θy+(1θ)x)θf(y)+(1θ)f(x)f(x)

    矛盾

    一些机器学习的example

    1. 线性回归

      J ( θ ) = ∣ ∣ W T Φ ( X ) − Y ∣ ∣ 2 2 = ∑ ( w t ϕ ( x i ) − y i ) 2 J(\theta) = ||W^T\Phi (X)-Y||_2^2=\sum (w^t\phi(x_i)-y_i)^2 J(θ)=WTΦ(X)Y22=(wtϕ(xi)yi)2
      因为 f i ( w ) = ( w t ϕ ( x i ) − y i ) 2 f_i(w)=(w^t\phi(x_i)-y_i)^2 fi(w)=(wtϕ(xi)yi)2 是一个复合affine 凸函数(i.e. x^2 是凸函数,而 w t ϕ ( x i ) − y i w^t\phi(x_i)-y_i wtϕ(xi)yi 是对 w w w 的affine function)
    2. logistic reg

      J ( θ ) = − ∑ y i log ⁡ g ( θ x i ) + ( 1 − y i ) log ⁡ 1 − g ( θ x i ) J(\theta)=-\sum y_i\log g(\theta x_i) + (1-y_i)\log 1- g(\theta x_i) J(θ)=yilogg(θxi)+(1yi)log1g(θxi)
    3. SVM

      SVM 的目标函数是一个 QP(quadratic opt) 问题, O ( n 3 ) O(n^3) O(n3)
      这个问题还引出一些凸问题有趣的性质,比如lagrange duality 和 KKT,我们后面分析

    lagrange & KKT

    lagrange mutiplier

    对于上面的凸优化问题来说,我们有它的lagrange function

    L ( x , α , β ) = f ( x ) + ∑ α i g i ( x ) + ∑ β i h i ( x ) s . t . α i &gt; = 0 \begin{aligned} \mathcal{L}(x,\alpha,\beta)&amp;=f(x)+\sum \alpha_i g_i(x) + \sum \beta_i h_i(x)\\ &amp;s.t. \alpha_i &gt;=0 \end{aligned} L(x,α,β)=f(x)+αigi(x)+βihi(x)s.t.αi>=0

    其中 x x x 称为 primal variable, α , β \alpha,\beta α,β 是dual variable

    NOTE 其实可以将 α , β \alpha,\beta α,β 理解成打破原来约束的代价

    primal & dual 原问题与对偶问题

    中间那段 english 不用看,直接看形式就行了

    我们下面试图证明这样一个弱对偶性(weak duality)

    d ∗ = θ D ( α ∗ , β ∗ ) ≤ p ∗ = θ P ( x ∗ ) d^*=\theta_D(\alpha^*,\beta^*)\le p^*=\theta_P(x^*) d=θD