slam十四讲ch4

文章目录

    • ch4-主要内容推导 + 课后习题 [十四讲]
        • 主要目标
        • 4.1 群
        • 4.2 李群与李代数
        • 4.3 指数映射和对数映射
        • 4.4 李代数求导与扰动模型
        • 实践:Sophus库的使用
      • 2.课后习题:

ch4-主要内容推导 + 课后习题 [十四讲]

简略框图

slam十四讲ch4_第1张图片

主要目标

1.理解群与李代数的概念,SO3、SE3及其对应李代数的表示方式。

2.理解BCH近似的意义【BCH公式-贯穿】

3.学会在李代数上的扰动模型【雅克比求导】

4.学会sophus对李代数进行运算。【sophus库】

​ 旋转矩阵本身带有约束(正交且行列式为1)。优化时会引入额外约束。 通过李群-李代数间转换关系,希望把位姿估计变成无约束的优化问题。【通过转换成李代数,去除额外约束】

4.1 群

  • 三维旋转矩阵构成 特殊正交群

S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n) = \{R\in \mathbb{R}^{n\times n} |RR^T=I,det(R)=1\} SO(n)={RRn×nRRT=I,det(R)=1}

  • 三维变换矩阵构成 特殊欧式群

S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \{ T =\left[\begin{array}{cc} R& t \\0^T&1\end{array} \right] \in \mathbb{R}^{4\times 4} | R\in SO(3), t\in \mathbb{R}^3 \} SE(3)={T=[R0Tt1]R4×4RSO(3),tR3}

定义:群(Group)是一种集合加上一种运算的代数结构。

记集合为A,运算为 ⋅ \cdot ,那么当运算满足以下性质,称 ( A , ⋅ ) (A,\cdot) (A,) 成群: 群的条件:封结幺逆-“凤姐咬你“

1.封闭性: ∀ a 1 , a 2 ∈ A ,     a 1 + a 2 ∈ A \forall a_1,a_2 \in A, \ \ \ a_1 + a_2 \in A a1,a2A   a1+a2A

2.结合律: ∀ a 1 , a 2 , a 3 ∈ A ,     ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1,a_2,a_3 \in A, \ \ \ (a_1\cdot a_2)\cdot a_3 = a_1\cdot (a_2\cdot a_3) a1,a2,a3A,   (a1a2)a3=a1(a2a3)

3.幺元: ∃ a 0 ∈ A ,     s . t .     ∀ a ∈ A ,     a 0 ⋅ a = a ⋅ a 0 = a \exists a_0\in A, \ \ \ s.t. \ \ \ \forall a\in A, \ \ \ a_0 \cdot a = a \cdot a_0 = a a0A,   s.t.   aA,   a0a=aa0=a

4.逆: ∀ a ∈ A ,     ∃ a − 1 ∈ A ,     s . t .    a ⋅ a − 1 = a 0 \forall a \in A, \ \ \ \exists a^{-1}\in A, \ \ \ s.t. \ \ a\cdot a^{-1} = a_0 aA,   a1A,   s.t.  aa1=a0

旋转矩阵、变换矩阵与矩阵乘法构成群。

在这里插入图片描述

4.2 李群与李代数

李群(Lie Group):

  • 具有连续(光滑)性质的群。
  • 既是群也是流形。
  • 直观上,一个刚体能够连续地在空间中运动,故SO(3)和SE(3)都是李群。
  • 但是,SO(3) 和 SE(3) 只有定义良好的乘法,没有加法,所以难以进行取极限、求导等操作。

李代数 : 与李群对应的一种结构,位于向量空间。

  • 通常记作小写的so(3)和se(3)。书中以哥特体突出显示。
  • 事实上是李群单位元处的正切空间

李代数的引出

考虑任意旋转R,满足 R R T = I RR^T=I RRT=I . 令R随时间变化(连续运动),有 R ( t ) R ( t ) T = I R(t)R(t)^T = I R(t)R(t)T=I .

  • 两侧对时间求导:

R ˙ ( t ) R ( t ) T + R ( t ) R ˙ ( t ) T = 0 R ˙ ( t ) R ( t ) T = − ( R ˙ ( t ) R ( t ) T ) T \dot{R}(t)R(t)^T + R(t)\dot{R}(t)^T= 0 \\ \dot{R}(t)R(t)^T = -(\dot{R}(t)R(t)^T)^T R˙(t)R(t)T+R(t)R˙(t)T=0R˙(t)R(t)T=(R˙(t)R(t)T)T

  • [ ϕ ( t ) ∧ \phi(t)^\wedge ϕ(t)]很明显,这是一个反对称矩阵,记 R ˙ ( t ) R ( t ) T = ϕ ( t ) ∧ \dot{R}(t)R(t)^T = \phi(t)^\wedge R˙(t)R(t)T=ϕ(t)

    两侧右乘R(t) : R ˙ ( t ) = ϕ ( t ) ∧ R ( t ) \Large{\dot{R}(t) = \phi(t)^\wedge R(t)} R˙(t)=ϕ(t)R(t)

    单位元附近: t 0 = 0 ,     R ( 0 ) = I t_0 = 0, \ \ \ R(0) = I t0=0,   R(0)=I公式样式识别不了,就只能贴图了
    在这里插入图片描述

    ϕ \phi ϕ 反映了一阶导数性质,位于正切空间(tangent space)上。

    在  附近,假设  不变,有微分方程: $\dot{R}(t) = \phi(t_0)^\wedge R(t) = \phi_0^\wedge R(t)  $

    已知初始情况: R ( 0 ) = I R(0) = I R(0)=I ,解之,得: R ( t ) = e x p ( ϕ 0 ∧ t ) R(t) = exp(\phi_0^\wedge t) R(t)=exp(ϕ0t)

    • [引伸] [ w ( t ) ∧ w(t)^\wedge w(t)] R T R = I R^TR = I RTR=I 转置互换位置。 对两侧求导

    R ˙ T R + R T R ˙ = 0 R T R ˙ = − ( R T R ˙ ) T \dot{R}^TR + R^T\dot{R} = 0 \\ R^T\dot{R} = -(R^T\dot{R} )^T R˙TR+RTR˙=0RTR˙=(RTR˙)T

    ​ 令 R T R ˙ = w ∧ R^T \dot{R} = w^\wedge RTR˙=w , ∴ R ˙ = R w ∧ \Large{\therefore \dot{R} = Rw^\wedge} R˙=Rw

R ( t ) = e x p ( ϕ 0 ∧ t ) R(t) = exp(\phi_0^\wedge t) R(t)=exp(ϕ0t) 该式说明,对任意t,都可以找到一个R和一个 的对应关系;该关系称为指数映射(Exponential Map);这里的 称为SO(3)对应的李代数: so(3)

李代数的定义:每个李群都有与之对应的李代数。李代数描述了李群单位元附近的正切空间
性质。slam十四讲ch4_第2张图片

​ 二元运算[,]被称为李括号(Lie Bracket). 直观上,李括号表达了两个元素的差异。

**例子:**三维空间向量 + 叉乘 构成了李代数so(3):
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 李 括 号 [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ \mathfrak{so}(3) = \{ \phi\in \mathbb{R}^3, \Phi = \phi^\wedge \in \mathbb{R}^{3\times 3} \} \\ \Phi = \phi^\wedge = \left [\begin{array}{cc} 0 &-\phi_3&\phi_2 \\ \phi_3 & 0 &-\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{array}\right] \in \mathbb{R}^{3\times3} \\ 李括号[\phi_1,\phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^\vee so(3)={ϕR3,Φ=ϕR3×3}Φ=ϕ=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ10R3×3[ϕ1,ϕ2]=(Φ1Φ2Φ2Φ1)
​ 同理,SE(3)对应的李代数se(3):
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s e ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{se}(3) = \left\{ \xi = \left [\begin{array}{cc} \rho \\ \phi \end{array}\right] \in \mathbb{R}^6 , \rho\in \mathbb{R}^3, \phi \in \mathfrak{se}(3), \xi^\wedge = \left [\begin{array}{cc} \phi^\wedge & \rho \\ 0^T&0 \end{array}\right] \in \mathbb{R}^{4\times 4} \right\} se(3)={ξ=[ρϕ]R6,ρR3,ϕse(3),ξ=[ϕ0Tρ0]R4×4}
​ 注意,这里的^不再是反对称矩阵,但仍保留记法:
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 李 括 号 [ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ \xi^\wedge = \left [\begin{array}{cc} \phi^\wedge & \rho \\ 0^T&0 \end{array}\right] \in \mathbb{R}^{4\times 4} \\李括号[\xi_1,\xi_2] = (\xi_1^\wedge \xi_2^\wedge - \xi_2^\wedge\xi_1^\wedge)^\vee ξ=[ϕ0Tρ0]R4×4[ξ1,ξ2]=(ξ1ξ2ξ2ξ1)

4.3 指数映射和对数映射

4.3.1 so(3) 到 SO(3)的指数映射【旋转矩阵R】

指数映射:李代数->李群: R ( t ) = e x p ( ϕ ∧ ) R(t) = exp(\phi^\wedge ) R(t)=exp(ϕ)

但是 是一个矩阵,对于矩阵,如何定义求指数运算?
e x p ( ϕ ∧ ) = ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n exp(\phi^\wedge) = \sum_{n=0}^\infty\frac1{n!}(\phi^\wedge)^n exp(ϕ)=n=0n!1(ϕ)n
由于 ϕ \phi ϕ 是向量, 先定义其角度和模长: ϕ = θ a \phi = \theta a ϕ=θa .关于 a a a,有以下性质:(为化解Taylor展开式中的高阶项)
a ∧ a ∧ = a a T − I , a ∧ a ∧ a ∧ = − a ∧ a^\wedge a^\wedge = aa^T - I, \\a^\wedge a^\wedge a^\wedge = -a^\wedge aa=aaTI,aaa=a
slam十四讲ch4_第3张图片
结果等于ch2中的罗德里格斯公式,说明so(3)的物理意义就是旋转向量。 反之,给定旋转矩阵,也能求李代数。
在这里插入图片描述

4.3.2 SO(3) 到 se(3)的对数映射
ϕ = l n ( R ) ∨ = ( ∑ n = 0 ∞ ( − 1 ) n n + 1 ( R − I ) ( n + 1 ) ) ∨ \phi = ln(R)^\vee = \left ( \sum^\infty_{n=0}\frac{(-1)^n}{n+1} (R-I)^{(n+1)} \right)^\vee ϕ=ln(R)=(n=0n+1(1)n(RI)(n+1))
但既然明白了so(3)物理意义=旋转向量,旋转矩阵R->so(3)也可利用这个关系。

4.3.3 se(3) 到 SE(3)的指数映射
slam十四讲ch4_第4张图片

SE(3)到 se(3)的对数映射 部分同上。总结关系如下:

slam十四讲ch4_第5张图片

4.4 李代数求导与扰动模型

SLAM需要进行位姿估计,但李群无法进行加法( R 1 + R 2 ∉ S O ( 3 ) R_1+R_2 \notin SO(3) R1+R2/SO(3)) ,导数无从定义。

解决方法

  1. 利用李代数上的加法定义李群的导数?
  2. 利用指数映射和对数映射完成变换关系。

新的问题: 李代数上的加法,是否等价与李群上的乘法?
e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) = ?   e x p ( ( ϕ 1 + ϕ 2 ) ∧ ) exp(\phi^\wedge_1)exp(\phi^\wedge_2) =? \ exp((\phi_1+\phi_2)^\wedge) exp(ϕ1)exp(ϕ2)=? exp((ϕ1+ϕ2))
如果参数为标量,则该式成立。 但 ϕ ∧ \phi^\wedge ϕ 为矩阵!两个李代数指数映射乘积的完整形式由BCH(Baker-Campbell-Hausdorff)公式给出:[完整形式可参考wiki,形式非常复杂,这里只给出部分展开式](方括号为李括号)
l n ( e x p ( A ) e x p ( B ) ) = A + B + 1 2 [ A , B ] + 1 12 [ A , [ A , B ] ] − 1 12 [ B , [ A , B ] ] + . . . ln(exp(A)exp(B)) = A + B + \frac12[A,B] + \frac1{12}[A,[A,B]] -\frac1{12}[B,[A,B]] + ... ln(exp(A)exp(B))=A+B+21[A,B]+121[A,[A,B]]121[B,[A,B]]+...

  • 当其中一个量为小量时,忽略其高阶项,BCH给出线性近似形式如下:

l n ( e x p ( ϕ 1 ∧ ) e x p ( ϕ 2 ∧ ) ) ∨ ≈ { J l ( ϕ 2 ) − 1 ϕ 1 + ϕ 2 i f   ϕ 1   i s   s m a l l , J r ( ϕ 1 ) − 1 ϕ 2 + ϕ 1 i f   ϕ 2   i s   s m a l l , ln(exp(\phi^\wedge_1)exp(\phi^\wedge_2))^\vee \approx \left \{ \begin{array}{rcl} J_l(\phi_2)^{-1}\phi_1 + \phi_2 & if \ \phi_1 \ is \ small, \\J_r(\phi_1)^{-1}\phi_2 + \phi_1 & if \ \phi_2 \ is \ small, \end{array}\right. ln(exp(ϕ1)exp(ϕ2)){Jl(ϕ2)1ϕ1+ϕ2Jr(ϕ1)1ϕ2+ϕ1if ϕ1 is small,if ϕ2 is small,

​ 这里的
slam十四讲ch4_第6张图片

  • 直观写法(以左乘为例):

e x p ( Δ ϕ ∧ ) e x p ( ϕ ∧ ) = e x p ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) ∧ ) exp(\Delta \phi^\wedge)exp(\phi^\wedge) = exp((\phi + J_l^{-1}(\phi)\Delta\phi )^\wedge) exp(Δϕ)exp(ϕ)=exp((ϕ+Jl1(ϕ)Δϕ))

​ 在李群上左乘小量时,李代数上的加法相差左雅克比的逆。[1.李群左/右乘小量 2.对应的李代数]

​ 反之:[1.在对应的李代数上加小量,2.也相应的左/右乘雅克比*小量]
e x p ( ( ϕ + Δ ϕ ) ∧ ) = e x p ( ( J l Δ ϕ ) ∧ ) e x p ( ϕ ∧ ) = e x p ( ϕ ∧ ) e x p ( ( J r Δ ϕ ) ∧ ) exp((\phi + \Delta\phi )^\wedge) = exp( (J_l\Delta\phi)^\wedge) exp(\phi^\wedge) = exp(\phi^\wedge) exp((J_r\Delta\phi)^\wedge) exp((ϕ+Δϕ))=exp((JlΔϕ))exp(ϕ)=exp(ϕ)exp((JrΔϕ))

  • SE(3) 比 SO(3) 更复杂:[下面的 J \mathcal{J} J 形式比较复杂,故不展开]

e x p ( Δ ξ ∧ ) e x p ( ξ ∧ ) ≈ e x p ( ( J l − 1 Δ ξ + ξ ) ∧ ) e x p ( ξ ∧ ) e x p ( Δ ξ ∧ ) ≈ e x p ( ( J r − 1 Δ ξ + ξ ) ∧ ) exp(\Delta\xi^\wedge) exp(\xi^\wedge) \approx exp( (\mathcal{J}_l^{-1 }\Delta\xi + \xi)^\wedge ) \\exp(\xi^\wedge) exp(\Delta\xi^\wedge) \approx exp( (\mathcal{J}_r^{-1 }\Delta\xi + \xi)^\wedge ) exp(Δξ)exp(ξ)exp((Jl1Δξ+ξ))exp(ξ)exp(Δξ)exp((Jr1Δξ+ξ))

4.4.2 求导和扰动模型

​ 通过BCH线性近似,可以定义李代数上的导数。 针对我们的问题:旋转后的点关于旋转的导数:不严谨的写作 求 导 ( R p ) 求 导 R \frac{求导(Rp)}{求导R} R(Rp) . 由于旋转矩阵R没有加法,导数无从定义。

解决

  • 对R对应的李代数加上小量,求相对于小量的变化率(导数模型).[先求李代数,在加小量]
  • 对R左乘或右乘一个小量,再求相对于小量的李代数的变换率(扰动模型).[先乘小量,再求李代数]

导数模型:李群->李代数,在加小量(加法能成立,也是因为李代数的原因)
slam十四讲ch4_第7张图片
​ 缺点:就是雅克比计算
slam十四讲ch4_第8张图片

小结

  • 利用BCH线性近似,能推导so(3) 和se(3)上的导数和扰动模型。
  • 通常情况下,扰动模型更为简洁实用。

实践:Sophus库的使用

  • 三维旋转R

    李群=R=旋转矩阵 ↔ 对 数 映 射 l o g 指 数 映 射 e x p \Large\leftrightarrow_{对数映射log}^{指数映射exp} logexp 李代数= ϕ \phi ϕ = 向量=旋转向量.

    李代数部分: hat=反对称矩阵[记下] ; vee=反对称矩阵到向量

    增量扰动模型的更新: exp(\delta) * R

	// hat 为向量到反对称矩阵【待记】
    std::cout<< "so3 hat = " <<Sophus::SO3d::hat(so3)<<std::endl;
    // 相对的,vee为反对称到向量 【待记】
    std::cout<< "so3 hat vee = " <<Sophus::SO3d::vee (Sophus::SO3d::hat(so3)).transpose()<<std::endl;
  • 三维变换T

    李群(4x4矩阵)与李代数(6x1向量)之间的变换同上。

    李代数部分: hat=反对称矩阵[记下] ; vee=反对称矩阵到向量【同上】

    增量扰动模型的更新: exp(\delta) * T

2.课后习题:

1.验证SO(3)、SE(3)、和Sim(3)关于乘法成群 [群的定义]
S O ( n ) = { R ∈ R n × n ∣ R R T = I , d e t ( R ) = 1 } S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } S i m ( 3 ) = { T s = [ s R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } ,     s 为 缩 放 因 子 SO(n) = \{R\in \mathbb{R}^{n\times n} |RR^T=I,det(R)=1\} \\ SE(3) = \{ T =\left[\begin{array}{cc} R& t \\0^T&1\end{array} \right] \in \mathbb{R}^{4\times 4} | R\in SO(3), t\in \mathbb{R}^3 \} \\ Sim(3) = \{ Ts =\left[\begin{array}{cc} sR& t \\0^T&1\end{array} \right] \in \mathbb{R}^{4\times 4} | R\in SO(3), t\in \mathbb{R}^3 \} , \ \ \ s为缩放因子 SO(n)={RRn×nRRT=I,det(R)=1}SE(3)={T=[R0Tt1]R4×4RSO(3),tR3}Sim(3)={Ts=[sR0Tt1]R4×4RSO(3),tR3},   s

群的条件:

1.封闭性: ∀ a 1 , a 2 ∈ A ,     a 1 + a 2 ∈ A \forall a_1,a_2 \in A, \ \ \ a_1 + a_2 \in A a1,a2A   a1+a2A

2.结合律: ∀ a 1 , a 2 , a 3 ∈ A ,     ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) \forall a_1,a_2,a_3 \in A, \ \ \ (a_1\cdot a_2)\cdot a_3 = a_1\cdot (a_2\cdot a_3) a1,a2,a3A,   (a1a2)a3=a1(a2a3)

3.幺元: ∃ a 0 ∈ A ,     s . t .     ∀ a ∈ A ,     a 0 ⋅ a = a ⋅ a 0 = a \exists a_0\in A, \ \ \ s.t. \ \ \ \forall a\in A, \ \ \ a_0 \cdot a = a \cdot a_0 = a a0A,   s.t.   aA,   a0a=aa0=a

4.逆: ∀ a ∈ A ,     ∃ a − 1 ∈ A ,     s . t .    a ⋅ a − 1 = a 0 \forall a \in A, \ \ \ \exists a^{-1}\in A, \ \ \ s.t. \ \ a\cdot a^{-1} = a_0 aA,   a1A,   s.t.  aa1=a0

SO(3): 1 2 满足 ; 幺元 R 0 = I R_0 = I R0=I ; R R − 1 = I RR^{-1} = I RR1=I

SE(3): 1 2 满足 ; 幺元 T 0 = I T_0= I T0=I ; T T − 1 = I TT^{-1} = I TT1=I
[ a b c d ] − 1 = 1 a d − b c [ d − b − c a ] ∴ T − 1 = 1 R [ 1 − t 0 T R ] = [ 1 R − t R 0 T 1 ] ∴ T T − 1 = I \left[\begin{array}{cc} a& b \\c&d\end{array} \right]^{-1} = \frac1{ad-bc}\left[\begin{array}{cc} d&-b \\ -c&a \end{array} \right] \\ \therefore T^{-1} = \frac1{R} \left[\begin{array}{cc} 1& -t \\0^T&R\end{array} \right] = \left[\begin{array}{cc} \frac 1R& \frac{-t}R \\0^T&1\end{array} \right] \\ \therefore TT^{-1} = I [acbd]1=adbc1[dcba]T1=R1[10TtR]=[R10TRt1]TT1=I
Sim(3):1 2 满足 ; 幺元 T s 0 = I Ts_0= I Ts0=I ; 同理,(s=1的时候) T s T s − 1 = I TsTs^{-1} = I TsTs1=I

2.验证 ( R 3 , R , × ) (\mathbb{R}^3, \mathbb{R}, \times) (R3,R,×)构成李代数. [李代数的定义]

slam十四讲ch4_第9张图片
1.实数集向量,实数x实数 = 实数 ; 2. ( a X + b Y ) × Z = a X × Z + b Y × Z (aX+bY)\times Z = aX\times Z + bY\times Z (aX+bY)×Z=aX×Z+bY×Z

  1. X × X = ∣ X ∣ 2 s i n < x , x > = 0 X\times X = |X|^2sin = 0 X×X=X2sin<x,x>=0 ;

  2. [ X , [ Y , Z ] ] + [ Z , [ X , Y ] ] + [ Y , [ Z , X ] ] = X × ( Y × Z ) + Z × ( X × Y ) + Y × ( Z × X ) 利 用 拉 格 朗 日 矢 量 公 式 = Y ∗ ( X ∗ Z ) − Z ( X ∗ Y ) + X ∗ ( Z ∗ Y ) − Y ∗ ( Z ∗ X ) + Z ∗ ( Y ∗ X ) − X ( Y ∗ Z ) = 0 [X,[Y,Z]] + [Z,[X,Y]] + [Y,[Z,X]] \\=X\times(Y\times Z) + Z\times(X\times Y)+Y\times(Z\times X) {利用拉格朗日矢量公式} \\= Y*(X*Z)-Z(X*Y) + X*(Z*Y) \\ -Y*(Z*X) + Z*(Y*X)-X(Y*Z) \\ = 0 [X,[Y,Z]]+[Z,[X,Y]]+[Y,[Z,X]]=X×(Y×Z)+Z×(X×Y)+Y×(Z×X)=Y(XZ)Z(XY)+X(ZY)Y(ZX)+Z(YX)X(YZ)=0

3.验证so(3) 和se(3)满足李代数要求的性质 [李代数的性质]

已知:

三维空间向量 + 叉乘 构成了李代数so(3):
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] ∈ R 3 × 3 李 括 号 [ ϕ 1 , ϕ 2 ] = ( Φ 1 Φ 2 − Φ 2 Φ 1 ) ∨ \mathfrak{so}(3) = \{ \phi\in \mathbb{R}^3, \Phi = \phi^\wedge \in \mathbb{R}^{3\times 3} \} \\ \Phi = \phi^\wedge = \left [\begin{array}{cc} 0 &-\phi_3&\phi_2 \\ \phi_3 & 0 &-\phi_1 \\ -\phi_2 & \phi_1 & 0 \end{array}\right] \in \mathbb{R}^{3\times3} \\ 李括号[\phi_1,\phi_2] = (\Phi_1\Phi_2 - \Phi_2\Phi_1)^\vee so(3)={ϕR3,Φ=ϕR3×3}Φ=ϕ=0ϕ3ϕ2ϕ30ϕ1ϕ2ϕ10R3×3[ϕ1,ϕ2]=(Φ1Φ2Φ2Φ1)
​ 同理,SE(3)对应的李代数se(3):
s e ( 3 ) = { ξ = [ ρ ϕ ] ∈ R 6 , ρ ∈ R 3 , ϕ ∈ s e ( 3 ) , ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } \mathfrak{se}(3) = \left\{ \xi = \left [\begin{array}{cc} \rho \\ \phi \end{array}\right] \in \mathbb{R}^6 , \rho\in \mathbb{R}^3, \phi \in \mathfrak{se}(3), \xi^\wedge = \left [\begin{array}{cc} \phi^\wedge & \rho \\ 0^T&0 \end{array}\right] \in \mathbb{R}^{4\times 4} \right\} se(3)={ξ=[ρϕ]R6,ρR3,ϕse(3),ξ=[ϕ0Tρ0]R4×4}
​ 注意,这里的^不再是反对称矩阵,但仍保留记法:
ξ ∧ = [ ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 李 括 号 [ ξ 1 , ξ 2 ] = ( ξ 1 ∧ ξ 2 ∧ − ξ 2 ∧ ξ 1 ∧ ) ∨ \xi^\wedge = \left [\begin{array}{cc} \phi^\wedge & \rho \\ 0^T&0 \end{array}\right] \in \mathbb{R}^{4\times 4} \\李括号[\xi_1,\xi_2] = (\xi_1^\wedge \xi_2^\wedge - \xi_2^\wedge\xi_1^\wedge)^\vee ξ=[ϕ0Tρ0]R4×4[ξ1,ξ2]=(ξ1ξ2ξ2ξ1)

【没展示全的部分,自己可以去推下】
slam十四讲ch4_第10张图片
3.2 验证:se(3)
slam十四讲ch4_第11张图片
4. 验证性质(4.20)和(4.21)
slam十四讲ch4_第12张图片

5.证明: R P ∧ R T = ( R p ) ∧ RP^\wedge R^T = (Rp)^\wedge RPRT=(Rp) [伴随性质]

6.证明: R e x p ( p ∧ ) R T = e x p ( ( R p ) ∧ ) Rexp(p^\wedge) R^T =exp( (Rp)^\wedge) Rexp(p)RT=exp((Rp)) [SO(3)的伴随性质]

T e x p ( ξ ∧ ) T T = e x p ( A d ( T ) ξ ) ∧ ) Texp(\xi^\wedge) T^T =exp( Ad(T)\xi)^\wedge) Texp(ξ)TT=exp(Ad(T)ξ)) [SE(3)的伴随性质]

其中:
A d ( T ) = [ R t ∧ R 0 R ] Ad(T) = \left[\begin{array}{cc} R&t^\wedge R \\ 0 & R \end{array}\right] Ad(T)=[R0tRR]


5.6.证明:
slam十四讲ch4_第13张图片

7. 仿照左扰动推导,推导SO(3)和SE(3)在右扰动下的导数 [扰动-右]
slam十四讲ch4_第14张图片

8. 搜索cmake的find_package指令是如何运作的。它有哪些可选参数? 为了让cmake找到某个库,需要哪些先决条件? [扰动-右]

来源:《cmake实践》

FIND_PACKAGE( [major.minor] [QUIET] [NO_MODULE] 	[[REQUIRED|COMPONENTS] [componets...]])

QUIET REQUIRED 的使用,以及自建Findxxx.cmake模块并加入工程。【基础的还是需要熟练掌握的,不常用的会查“字典”即可。】

一,使用 FindCURL 模块

​ 用来调用预定义在 CMAKE_MODULE_PATH 下的 Find.cmake 模块,你也可以自己定义 Find模块,通过 SET(CMAKE_MODULE_PATH dir)将其放入工程的某个目录中供工程使用。

FIND_PACKAGE(CURL)
IF(CURL_FOUND)
	INCLUDE_DIRECTORIES(${CURL_INCLUDE_DIR})
	TARGET_LINK_LIBRARIES(curltest ${CURL_LIBRARY})
ELSE(CURL_FOUND)
	MESSAGE(FATAL_ERROR ”CURL library not found”)
ENDIF(CURL_FOUND)

对于系统预定义的 Find.cmake模块,使用方法一般如上例所示:
每一个模块都会定义以下几个变量
_FOUND
_INCLUDE_DIR or _INCLUDES
_LIBRARY or _LIBRARIES

​ 你可以通过_FOUND 来判断模块是否被找到,如果没有找到,按照工程的需要关闭某些特性、给出提醒或者中止编译,上面的例子就是报出致命错误并终止构建。

​ 如果_FOUND为真,则将_INCLUDE_DIR加入INCLUDE_DIRECTORIES
_LIBRARY 加入 TARGET_LINK_LIBRARIES中。

  • 我们再来看一个复杂的例子,通过_FOUND来控制工程特性:
SET(mySources viewer.c)
SET(optionalSources)
SET(optionalLibs)
FIND_PACKAGE(JPEG)
IF(JPEG_FOUND)
	SET(optionalSources ${optionalSources} jpegview.c)
	INCLUDE_DIRECTORIES( ${JPEG_INCLUDE_DIR} )
	SET(optionalLibs ${optionalLibs} ${JPEG_LIBRARIES} )
	ADD_DEFINITIONS(-DENABLE_JPEG_SUPPORT)
ENDIF(JPEG_FOUND)
IF(PNG_FOUND)
	SET(optionalSources ${optionalSources} pngview.c)
	INCLUDE_DIRECTORIES( ${PNG_INCLUDE_DIR} )
	SET(optionalLibs ${optionalLibs} ${PNG_LIBRARIES} )
	ADD_DEFINITIONS(-DENABLE_PNG_SUPPORT)
ENDIF(PNG_FOUND)
ADD_EXECUTABLE(viewer ${mySources} ${optionalSources} )
TARGET_LINK_LIBRARIES(viewer ${optionalLibs}
# 通过判断系统是否提供了 JPEG 库来决定程序是否支持 JPEG 功能

二,编写属于自己的 FindHello 模块。

1,定义 cmake/FindHELLO.cmake 模块

FIND_PATH(HELLO_INCLUDE_DIR hello.h /usr/include/hello
/usr/local/include/hello) # names paths

FIND_LIBRARY(HELLO_LIBRARY NAMES hello PATH /usr/lib
/usr/local/lib)

IF (HELLO_INCLUDE_DIR AND HELLO_LIBRARY)
	SET(HELLO_FOUND TRUE)
ENDIF (HELLO_INCLUDE_DIR AND HELLO_LIBRARY)

IF (HELLO_FOUND)
	IF (NOT HELLO_FIND_QUIETLY) # QUIETLY
		MESSAGE(STATUS "Found Hello: ${HELLO_LIBRARY}")
	ENDIF (NOT HELLO_FIND_QUIETLY)
ELSE (HELLO_FOUND)
	IF (HELLO_FIND_REQUIRED) # REQUIRED
		MESSAGE(FATAL_ERROR "Could not find hello library")
	ENDIF (HELLO_FIND_REQUIRED)
ENDIF (HELLO_FOUND

针对上面的模块让我们再来回顾一下 FIND_PACKAGE 指令:

FIND_PACKAGE( [major.minor] [QUIET] [NO_MODULE]
	[[REQUIRED|COMPONENTS] [componets...]])

​ 前面的 CURL 例子中我们使用了最简单的 FIND_PACKAGE 指令,其实他可以使用多种参数,

QUIET 参数,对应与我们编写的 FindHELLO中的 HELLO_FIND_QUIETLY,如果不指定
这个参数,就会执行:

MESSAGE(STATUS "Found Hello: ${HELLO_LIBRARY}")

REQUIRED 参数,其含义是指这个共享库是否是工程必须的,如果使用了这个参数,说明这个链接库是必备库,如果找不到这个链接库,则工程不能编译。

对应于FindHELLO.cmake 模块中的 HELLO_FIND_REQUIRED 变量。同样,我们在上面的模块中定义了 HELLO_FOUND,HELLO_INCLUDE_DIR,HELLO_LIBRARY 变量供开发者在 FIND_PACKAGE指令中使用.


src/CMakeLists.txt文件,内容如下:

FIND_PACKAGE(HELLO)
IF(HELLO_FOUND)
	ADD_EXECUTABLE(hello main.c)
	INCLUDE_DIRECTORIES(${HELLO_INCLUDE_DIR})
	TARGET_LINK_LIBRARIES(hello ${HELLO_LIBRARY})
ENDIF(HELLO_FOUND)

为了能够让工程找到 FindHELLO.cmake 模块(存放在工程中的 cmake 目录)
我们在主工程文件 CMakeLists.txt 中加入:

SET(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)

三,使用自定义的 FindHELLO 模块构建工程
仍然采用外部编译的方式,建立 build 目录,进入目录运行:

cmake ..

我们可以从输出中看到:
Found Hello: /usr/lib/libhello.so
如果我们把上面的 FIND_PACKAGE(HELLO)修改为 FIND_PACKAGE(HELLO QUIET),则
不会看到上面的输出。
接下来就可以使用 make 命令构建工程,运行:
./src/hello 可以得到输出

Hello World。

说明工程成功构建。

四,如果没有找到 hello library 呢?
我们可以尝试将/usr/lib/libhello.x 移动到/tmp目录,这样,按照 FindHELLO模块
的定义,就找不到 hello library 了,我们再来看一下构建结果:

cmake ..

仍然可以成功进行构建,但是这时候是没有办法编译的。
修改 FIND_PACKAGE(HELLO)FIND_PACKAGE(HELLO REQUIRED),将 hello
library 定义为工程必须的共享库。
这时候再次运行 cmake …
我们得到如下输出:

CMake Error: Could not find hello library.

因为找不到 libhello.x,所以,整个 Makefile 生成过程被出错中止。

你可能感兴趣的:(SLAM十四讲讲,+,视觉SLAM课程,计算机视觉,矩阵,c++)