第三节课习题 Instructions

User Manual:

Open the PDF directly: View PDF PDF.
Page Count: 9

第三节课习题
深蓝学院讲师高翔
2018
年
6
月
1
日
1习题说明
•
第
i
节课习题所有材料打包在
Li.zip
中，∀i
=
1
.
.
.
8。
•
习题分为若干种：计算类习题，需要读者编程计算一个实际问题，我们会附有参考答案以供自测。
操作类习题，会指导读者做一个具体的实验，给出中间步骤截图或结果。简述类习题则提供阅读材
料，需要读者阅读材料后，回答若干问题。
•
每个习题会有一定的分值。每次习题分值加和为
10
分。你需要获得
8
分以上才能得到“通过”的
评价。带
∗
的习题为附加题，会在总分之外再提供一定的分值，所以总和可能超过
10
分。换句话
说，你也可以选择一道附加题，跳过一道正常题。
•
每道习题的给分由助教评判，简述类习题可能存在一定开放性，所以评分也存在主观因素。
•
请利用深蓝学院系统提交习题。每次习题我们会记通过与否。提交形式为
word
或
pdf
格式报告，
如有编程习题请提交可编译的源码。
•
为方便读者，我通常会准备一些阅读材料，放在
books/或
papers/目录下。请读者按个人需求使用
这些材料。它们多数是从网络下载的，如果侵犯到你的权利，请及时告诉我。
•
每个习题会标注大致用时，但视同学个人水平可能会有出入。
•
习题的完成情况会影响你对本课程内容的掌握程度，请认真、独立完成。习题总得分较高的同学将
获得推荐实习就业的资格，并有机会获得深蓝学院“旷视科技奖学金”和“悉见科技奖学金”。
1
2群的性质 (2 分，约 1小时)
课上我们讲解了什么是群。请根据群定义，求解以下问题：
1. {Z,+}是否为群？若是，验证其满足群定义；若不是，说明理由。
2. {N,+}是否为群？若是，验证其满足群定义；若不是，说明理由。
其中 Z为整数集，N为自然数集。
2
3验证向量叉乘的李代数性质 (2 分，约 1小时)
我们说向量和叉乘运算构成了李代数，现在请你验证它。书中对李代数的定义为：李代数由一个集合
V，一个数域 F和一个二元运算 [,]组成。如果它们满足以下几条性质，称 (V,F,[,]) 为一个李代数，记作
g。
1. 封闭性 ∀X,Y∈V,[X,Y]∈V.
2. 双线性 ∀X,Y,Z∈V, a, b ∈F,有：
[aX+bY,Z] = a[X,Z] + b[Y,Z],[Z, aX+bY] = a[Z,X] + b[Z,Y].
3. 自反性1∀X∈V,[X,X] = 0.
4. 雅可比等价 ∀X,Y,Z∈V,[X,[Y,Z]] + [Y,[Z,X]] + [Z,[X,Y]] = 0.
其中二元运算被称为李括号。
现取集合 V=R3，数域 F=R，李括号为：
[a,b] = a×b.(1)
请验证 g= (R3,R,×)构成李代数。
1自反性是指自己与自己的运算为零。
3
4推导 SE(3) 的指数映射 (2 分，约 1小时)
课上给出了 SO(3) 的指数映射推导，但对于 SE(3)，仅介绍了结论，没有给出详细推导。请你完成 SE(3)
指数映射部分，有关左雅可比的详细推导。
设ξ= [ρ,ϕ]T∈se(3)，它的指数映射为：
exp (ξ∧) =
∞
n=0
1
n!(ϕ∧)n∞
n=0
1
(n+1)! (ϕ∧)nρ
0T1
.(2)
令ϕ=θa，那么：
∞
n=0
1
(n+ 1)!(ϕ∧)n=sin θ
θI+1−sin θ
θaaT+1−cos θ
θa∧∆
=J.(3)
这也正是课件里提到的左雅可比。
提示：类比于 SO(3) 的泰勒展开，然后合并奇偶数项级数即得。
4
5伴随 (2 分，约 1小时)
在SO(3) 和SE(3) 上，有一个东西称为伴随（Adjoint）。下面请你证明 SO(3) 伴随的性质。
对于 SO(3)，有：
Rexp (p∧)RT=exp (Rp)∧.(4)
此时称 Ad(R) = R。
提示：首先你需要证明 ∀a∈R3,Ra∧RT= (Ra)∧，页面 https://math.stackexchange.com/questions/
2190603/derivation-of-adjoint-for-so3 提示了一种简洁的途径。
对于 SE(3)，有：
Texp(ξ∧)T−1=exp (Ad(T)ξ)∧(5)
其中 Ad(T)定义为：
Ad(T) = R t∧R
0R.(6)
这个性质将在后文的 Pose Graph 优化中用到。但是 SE(3) 的证明较为复杂，不作要求。
完整的 SO(3) 和SE(3) 性质见1和2。
5
表1: SO(3) 性质与其近似形式
李代数 李群 （左）雅可比
u∧=
u1
u2
u3
∧
=
0−u3u2
u30−u1
−u2u10
C=exp(ϕ∧)≡∞
n=0 1
n!(ϕ∧)n
≡cos ϕ1+ (1 −cos ϕ)aaT+sin ϕa∧
≈1+ϕ∧
J=1
0Cαdα≡∞
n=0 1
(n+1)! (ϕ∧)n
≡sin ϕ
ϕ1+ (1 −sin ϕ
ϕ)aaT+1−cos ϕ
ϕa∧
≈1+1
2ϕ∧
(αu+βv)∧≡αu∧+βv∧C−1≡CT≡∞
n=0 1
n!(−ϕ∧)n≈1−ϕ∧
J−1≡∞
n=0
Bn
n!(ϕ∧)n
≡ϕ
2cot ϕ
21+ (1 −ϕ
2cot ϕ
2)aaT−ϕ
2a∧
≈1−1
2ϕ∧
u∧T≡ −u∧ϕ=ϕaexp((ϕ+δϕ)∧)≈exp((Jδϕ)∧)exp(ϕ∧)
u∧v≡ −v∧u aTa≡1C≡1+ϕ∧J
u∧u≡0CTC≡1≡CCTJ(ϕ)≡CJ(−ϕ)
(W u)∧≡u∧(tr(W)1−W)−WTu∧tr(C)≡2cos ϕ+ 1 (exp(δϕ∧)C)α≈(1+ (A(α, ϕ)δϕ)∧)Cα
u∧v∧≡ −(uTv)1+vuTdet(C)≡1A(α, ϕ) = αJ(αϕ)J(ϕ)−1=∞
n=0
Fn(α)
n!(ϕ∧)n
u∧W v∧≡ −(−tr(vuT)1+vuT)
×(−tr(W)1+WT) + tr(WTvuT)1−WTvuTCa ≡a
u∧v∧u∧≡u∧u∧v∧+v∧u∧u∧+ (uTu)v∧Cϕ =ϕ
(u∧)3+ (uTu)u∧≡0Ca∧≡a∧C
u∧v∧v∧−v∧v∧u∧≡(v∧u∧v)∧Cϕ∧≡ϕ∧C
[u∧,v∧]≡u∧v∧−v∧u∧≡(u∧v)∧(Cu)∧≡Cu∧CT
[u∧,[u∧,...[u∧,
  
n
v∧]...]] ≡((u∧)nv)∧exp((Cu)∧)≡Cexp(u∧)CT
α, β ∈R,u,v,ϕ, δϕ∈R3,W,A,J∈R3×3,C∈SO(3)
6
表2: SE(3) 性质与其近似形式
李代数 李群 （左）雅可比
x∧=u
v∧
=v∧u
0T0
x⋏=u
v⋏
=v∧u∧
0v∧
(αx+βy)∧≡αx∧+βy∧
(αx+βy)⋏≡αx⋏+βy⋏
x⋏y≡ −y⋏x
x⋏x≡0
(x∧)4+vTv(x∧)2≡0
(x⋏)5+ 2 vTv(x⋏)3+vTv2(x⋏)≡0
[x∧,y∧]≡x∧y∧−y∧x∧≡(x⋏y)∧
[x⋏,y⋏]≡x⋏y⋏−y⋏x⋏≡(x⋏y)⋏
[x∧,[x∧,...[x∧,
  
n
y∧]...]] ≡((x⋏)ny)∧
[x⋏,[x⋏,...[x⋏,
  
n
y⋏]...]] ≡((x⋏)ny)⋏
p⊙=ε
η⊙
=η1−ε∧
0T0T
p⊚=ε
η⊚
=0ε
−ε∧0
x∧p≡p⊙x
pTx∧≡xTp⊚
ξ=ρ
ϕ
T=exp (ξ∧)≡∞
n=0 1
n!(ξ∧)n
≡1+ξ∧+1−cos ϕ
ϕ2(ξ∧)2+ϕ−sin ϕ
ϕ3(ξ∧)3
≈1+ξ∧
T≡C Jρ
0T1
ξ⋏≡ad (ξ∧)
T=exp (ξ⋏)≡∞
n=1 1
n!(ξ⋏)n
≡1+3sin ϕ−ϕcos ϕ
2ϕξ⋏+4−ϕsin ϕ−4cos ϕ
2ϕ2(ξ⋏)2
+sin ϕ−ϕcos ϕ
2ϕ3(ξ⋏)3+2−ϕsin ϕ−2cos ϕ
2ϕ4(ξ⋏)4
≈1+ξ⋏
T=Ad (T)≡C(Jρ)∧C
0C
tr (T)≡2cos ϕ+ 2,det (T)≡1
Ad (T1T2) = Ad (T1)Ad (T2)
T−1≡exp (−ξ∧)≡∞
n=1 1
n!(−ξ∧)n≈1−ξ∧
T−1≡CT−CTr
0T1
T−1≡exp (−ξ⋏)≡∞
n=1 1
n!(−ξ⋏)n≈1−ξ⋏
T−1≡CT−CT(Jρ)∧
0CT
Tξ≡ξ
T ξ∧≡ξ∧T,Tξ⋏≡ξ⋏T
(Tx)∧≡T x∧T−1,(Tx)⋏≡Tx⋏T−1
exp (Tx)∧≡Texp (x∧)T−1
exp (Tx)⋏≡Texp (x⋏)T−1
(T p)⊙≡T p⊙T−1
(T p)⊙T(T p)⊙≡T−Tp⊙Tp⊙T−1
J=1
0Tαdα≡∞
n=0 1
(n+1)! (ξ⋏)n
=1+4−ϕsin ϕ−4cos ϕ
2ϕ2ξ⋏+4ϕ−5sin ϕ+ϕcos ϕ
2ϕ3(ξ⋏)2
+2−ϕsin ϕ−2cos ϕ
2ϕ4(ξ⋏)3+2ϕ−3sin ϕ+ϕcos ϕ
2ϕ5(ξ⋏)4
≈1+1
2ξ⋏
J≡J Q
0J
J−1≡∞
n=0
Bn
n!(ξ⋏)n≈1−1
2ξ⋏
J−1≡J−1−J−1QJ−1
0J−1
Q=∞
n=0 ∞
m=0 1
(n+m+2)! (ϕ∧)nρ∧(ϕ∧)m
≡1
2ρ∧+ϕ−sin ϕ
ϕ3(ϕ∧ρ∧+ρ∧ϕ∧+ϕ∧ρ∧ϕ∧)
+ϕ2+2 cos ϕ−2
2ϕ4(ϕ∧ϕ∧ρ∧+ρ∧ϕ∧ϕ∧−3ϕ∧ρ∧ϕ∧)
+2ϕ−3sin ϕ+ϕcos ϕ
2ϕ5(ϕ∧ρ∧ϕ∧ϕ∧+ϕ∧ϕ∧ρ∧ϕ∧)
exp (ξ+δξ)∧≈exp (Jδξ)∧exp (ξ∧)
exp (ξ+δξ)⋏≈exp (Jδξ)⋏exp (ξ⋏)
T≡1+ξ⋏J
Jξ⋏≡ξ⋏J
J(ξ)≡T J (−ξ)
(exp (δξ∧)T)α≈1+ (A(α, ξ)δξ)∧Tα
A(α, ξ) = αJ(αξ)J(ξ)−1=∞
n=0
Fn(α)
n!(ξ⋏)n
α, β ∈R,u,v,ϕ, δϕ∈R3,p∈R4,x,y,ξ, δξ∈R6,C∈SO(3),J,Q∈R3×3,T,T1,T2∈SE(3),T∈Ad (SE(3)) ,J,A∈R6×6
7
6轨迹的描绘 (2 分，约 1小时)
我们通常会记录机器人的运动轨迹，来观察它的运动是否符合预期。大部分数据集都会提供标准轨迹
以供参考，如 kitti、TUM-RGBD 等。这些文件会有各自的格式，但首先你要理解它的内容。记世界坐标
系为 W，机器人坐标系为 C，那么机器人的运动可以用 TW C 或TCW 来描述。现在，我们希望画出机器
人在世界当中的运动轨迹，请回答以下问题：
1. 事实上，TW C 的平移部分即构成了机器人的轨迹。它的物理意义是什么？为何画出 TW C 的平移
部分就得到了机器人的轨迹？
2. 我为你准备了一个轨迹文件（code/trajectory.txt）。该文件的每一行由若干个数据组成，格式为
[t, tx, ty, tz, qx, qy, qz, qw],
其中 t为时间，tx, ty, tz为TW C 的平移部分，qx, qy, qz, qw是四元数表示的 TW C 的旋转部分，qw
为四元数实部。同时，我为你提供了画图程序 draw_trajectory.cpp 文件。该文件提供了画图部分
的代码，请你完成数据读取部分的代码，然后书写 CMakeLists.txt 以让此程序运行起来。注意我
们需要用到 Pangolin 库来画图，所以你需要事先安装 Pangolin（如果你做了第一次作业，那么现
在已经安装了）。CMakeLists.txt 可以参照 ORB-SLAM2 部分。
8
7*轨迹的误差 (2 分，约 1小时)
本题为附加题。
除了画出真实轨迹以外，我们经常需要把 SLAM 估计的轨迹与真实轨迹相比较。下面说明比较的原
理，请你完成比较部分的代码实现。
设真实轨迹（ground-truth）为 Tg，估计轨迹 Te。它们都以 TW C 的形式存储，格式同上题。现在，你
需要计算估计轨迹的误差。我们假设每一个 Tg都与给定的 Te对应。那么，对于任意第 i个位姿，它的误
差可定义为：
ei=∥log(T−1
gi Tei)∨∥2.(7)
即两个位姿之差的李代数二范数。于是，可以定义两条轨迹的均方根（Root-Mean-Square-Error, RMSE）
误差为：
RMSE(g, e) =
1
n
n
i=1
e2
i.(8)
我为你准备了 code/ground-truth.txt 和code/estimate.txt 两条轨迹。请你根据上面公式，实现 RMSE
的计算代码，给出最后的 RMSE 结果。作为验算，参考答案为：2.207。
注：
1. 公式 (7) 满足度量的定义：非负性、同一性、对称性、三角不等式，故形成距离函数。类似的，可
以定义 SO(3) 上的距离为：
d(R1,R2) =
ln R−1
1R2∨
2.
关于距离定义可以参见拓扑学或者泛函教材。
2. 实际当中的轨迹比较还要更复杂一些。通常 ground-truth 由其他传感器记录（如 vicon），它的采
样频率通常高于相机的频率，所以在处理之前还需要按照时间戳对齐。另外，由于传感器坐标系不
一致，还需要计算两个坐标系之间的差异。这件事也可以用 ICP 解得，我们将在后面的课程中讲
到。
3. 你可以用上题的画图程序将两条轨迹画在同一个图里，看看它们相差多少。
9

Navigation menu