slam十四讲ch4

文章目录

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

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

简略框图

主要目标

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)={R∈Rn×n∣RRT=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×4∣R∈SO(3),t∈R3}

定义：群(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,a2∈A，   a1+a2∈A

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,a3∈A,   (a1⋅a2)⋅a3=a1⋅(a2⋅a3)

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 ∃a0∈A,   s.t.   ∀a∈A,   a0⋅a=a⋅a0=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 ∀a∈A,   ∃a−1∈A,   s.t.  a⋅a−1=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)上。

已知初始情况： 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(ϕ0∧t)
- [引伸] [ 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(ϕ0∧t) 该式说明，对任意t，都可以找到一个R和一个的对应关系；该关系称为指数映射（Exponential Map）；这里的称为SO(3)对应的李代数： so(3)

李代数的定义：每个李群都有与之对应的李代数。李代数描述了李群单位元附近的正切空间
性质。

二元运算[,]被称为李括号(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−ϕ10⎦⎤∈R3×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=0∑∞n!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 a∧a∧=aaT−I,a∧a∧a∧=−a∧

结果等于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=0∑∞n+1(−1)n(R−I)(n+1))∨
但既然明白了so(3)物理意义=旋转向量，旋转矩阵R->so(3)也可利用这个关系。

4.3.3 se(3) 到 SE(3)的指数映射

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

4.4 李代数求导与扰动模型

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

解决方法：

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

新的问题：李代数上的加法，是否等价与李群上的乘法？
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,

这里的

直观写法（以左乘为例）：

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((ϕ+Jl−1(ϕ)Δϕ)∧)

在李群上左乘小量时，李代数上的加法相差左雅克比的逆。[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((Jl−1Δξ+ξ)∧)exp(ξ∧)exp(Δξ∧)≈exp((Jr−1Δξ+ξ)∧)

4.4.2 求导和扰动模型

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

解决：

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

导数模型：李群->李代数，在加小量(加法能成立，也是因为李代数的原因)

缺点：就是雅克比计算

小结：

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

实践：Sophus库的使用

三维旋转R

李群=R=旋转矩阵 ↔ 对数映射 l o g 指数映射 e x p \Large\leftrightarrow_{对数映射log}^{指数映射exp} ↔对数映射log指数映射exp 李代数= ϕ \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)={R∈Rn×n∣RRT=I,det(R)=1}SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3}Sim(3)={Ts=[sR0Tt1]∈R4×4∣R∈SO(3),t∈R3}, 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,a2∈A，   a1+a2∈A

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,a3∈A,   (a1⋅a2)⋅a3=a1⋅(a2⋅a3)

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 ∃a0∈A,   s.t.   ∀a∈A,   a0⋅a=a⋅a0=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 ∀a∈A,   ∃a−1∈A,   s.t.  a⋅a−1=a0

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

SE(3): 1 2 满足；幺元 T 0 = I T_0= I T0=I ; T T − 1 = I TT^{-1} = I TT−1=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=ad−bc1[d−c−ba]∴T−1=R1[10T−tR]=[R10TR−t1]∴TT−1=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 TsTs−1=I

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

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

X × X = ∣ X ∣ 2 s i n < x , x > = 0 X\times X = |X|^2sin<x,x> = 0 X×X=∣X∣2sin<x,x>=0 ;
[ 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∗(X∗Z)−Z(X∗Y)+X∗(Z∗Y)−Y∗(Z∗X)+Z∗(Y∗X)−X(Y∗Z)=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−ϕ10⎦⎤∈R3×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∧)∨

【没展示全的部分，自己可以去推下】

3.2 验证：se(3)

4. 验证性质(4.20)和(4.21)

5.证明: R P ∧ R T = ( R p ) ∧ RP^\wedge R^T = (Rp)^\wedge RP∧RT=(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)=[R0t∧RR]

5.6.证明：

7. 仿照左扰动推导，推导SO(3)和SE(3)在右扰动下的导数 [扰动-右]

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

来源：《cmake实践》

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

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

一，使用 FindCURL 模块

用来调用预定义在 CMAKE_MODULE_PATH 下的 Find<name>.cmake 模块，你也可以自己定义 Find<name>模块，通过 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<name>.cmake模块，使用方法一般如上例所示：
每一个模块都会定义以下几个变量
• <name>_FOUND
• <name>_INCLUDE_DIR or <name>_INCLUDES
• <name>_LIBRARY or <name>_LIBRARIES

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

如果<name>_FOUND为真，则将<name>_INCLUDE_DIR加入INCLUDE_DIRECTORIES，
将<name>_LIBRARY 加入 TARGET_LINK_LIBRARIES中。

我们再来看一个复杂的例子，通过<name>_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 pathsFIND_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) # QUIETLYMESSAGE(STATUS "Found Hello: ${HELLO_LIBRARY}")ENDIF (NOT HELLO_FIND_QUIETLY)
ELSE (HELLO_FOUND)IF (HELLO_FIND_REQUIRED) # REQUIREDMESSAGE(FATAL_ERROR "Could not find hello library")ENDIF (HELLO_FIND_REQUIRED)
ENDIF (HELLO_FOUND

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

FIND_PACKAGE(<name> [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 生成过程被出错中止。