简略框图
1.理解群与李代数的概念,SO3、SE3及其对应李代数的表示方式。
2.理解BCH近似的意义【BCH公式-贯穿】
3.学会在李代数上的扰动模型【雅克比求导】
4.学会sophus对李代数进行运算。【sophus库】
旋转矩阵本身带有约束(正交且行列式为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
旋转矩阵、变换矩阵与矩阵乘法构成群。
李群(Lie Group):
李代数 : 与李群对应的一种结构,位于向量空间。
李代数的引出:
考虑任意旋转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)
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.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)也可利用这个关系。
SE(3)到 se(3)的对数映射 部分同上。总结关系如下:
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]]+...
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Δϕ)∧)
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=旋转矩阵 ↔ 对 数 映 射 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
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 , [ 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]
7. 仿照左扰动推导,推导SO(3)和SE(3)在右扰动下的导数 [扰动-右]
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
模块,你也可以自己定义 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
模块,使用方法一般如上例所示:
每一个模块都会定义以下几个变量
•
•
•
你可以通过
来判断模块是否被找到,如果没有找到,按照工程的需要关闭某些特性、给出提醒或者中止编译,上面的例子就是报出致命错误并终止构建。
如果
为真,则将
加入INCLUDE_DIRECTORIES
,
将
加入 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 生成过程被出错中止。