第三节课习题 Instructions

User Manual:

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

Download
Open PDF In Browser	View PDF

第三节课习题
深蓝学院讲师高翔
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],

3. 自反性1

[Z, aX + bY ] = a[Z, X] + b[Z, Y ].

∀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.
请验证 g = (R3 , R, ×) 构成李代数。

1

自反性是指自己与自己的运算为零。

3

(1)

4 推导 SE(3) 的指数映射 (2 分，约 1 小时)
课上给出了 SO(3) 的指数映射推导，但对于 SE(3)，仅介绍了结论，没有给出详细推导。请你完成 SE(3)
指数映射部分，有关左雅可比的详细推导。
设 ξ = [ρ, ϕ]T ∈ se(3)，它的指数映射为：
∑
∞
exp (ξ ∧ ) = n=0

1
∧ n
n! (ϕ )

0T

∞
∑
n=0



1
∧ n
(n+1)! (ϕ ) ρ

.

(2)

1

令 ϕ = θa，那么：
(
)
sin θ
sin θ
1 − cos θ ∧ ∆
1
n
(ϕ∧ ) =
I + 1−
aaT +
a = J.
(n + 1)!
θ
θ
θ
n=0
∞
∑

这也正是课件里提到的左雅可比。
提示：类比于 SO(3) 的泰勒展开，然后合并奇偶数项级数即得。

4

(3)

5 伴随 (2 分，约 1 小时)
在 SO(3) 和 SE(3) 上，有一个东西称为伴随（Adjoint）。下面请你证明 SO(3) 伴随的性质。
对于 SO(3)，有：

(
∧)
R exp (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)，有：

(
∧)
T exp(ξ ∧ )T −1 = exp (Ad(T )ξ)

其中 Ad(T ) 定义为：

[
Ad(T ) =

R

t∧ R

0

R

]
.

这个性质将在后文的 Pose Graph 优化中用到。但是 SE(3) 的证明较为复杂，不作要求。
完整的 SO(3) 和 SE(3) 性质见1和2。

5

(5)

(6)

6

−u3
0
u1

n

1
∧ n
n=0 n! (−ϕ )

∑∞
≈ 1 − ϕ∧

Cϕ = ϕ
Ca∧ ≡ a∧ C
Cϕ∧ ≡ ϕ∧ C
(Cu)∧ ≡ Cu∧ C T
exp((Cu)∧ ) ≡ C exp(u∧ )C T

Ca ≡ a

ϕ = ϕa
aT a ≡ 1
C T C ≡ 1 ≡ CC T
tr(C) ≡ 2 cos ϕ + 1
det(C) ≡ 1

C −1 ≡ C T ≡

李群
∑
1
∧ n
C = exp(ϕ∧ ) ≡ ∞
n=0 n! (ϕ )
≡ cos ϕ1 + (1 − cos ϕ)aaT + sin ϕa∧
≈ 1 + ϕ∧
0
sin ϕ
1
ϕ

J=

∫1

（左）雅可比
∑
1
∧ n
≡ ∞
n=0 (n+1)! (ϕ )

C α dα

≡

ϕ ∧
+ (1 − sinϕ ϕ )aaT + 1−cos
a
ϕ
1 ∧
≈ 1 + 2ϕ
∑
Bn
∧ n
J −1 ≡ ∞
n=0 n! (ϕ )
ϕ
ϕ
ϕ
T − ϕ a∧
≡ 2 cot 2 1 + (1 − ϕ
cot
)aa
2
2
2
1 ∧
≈ 1 − 2ϕ
exp((ϕ + δϕ)∧ ) ≈ exp((J δϕ)∧ ) exp(ϕ∧ )
C ≡ 1 + ϕ∧ J
J (ϕ) ≡ CJ (−ϕ)
(exp(δϕ∧ )C)α ≈ (1 + (A(α, ϕ)δϕ)∧ )C α
∑
Fn (α)
A(α, ϕ) = αJ (αϕ)J (ϕ)−1 = ∞
(ϕ∧ )n
n=0
n!

α, β ∈ R, u, v, ϕ, δϕ ∈ R3 , W , A, J ∈ R3×3 , C ∈ SO(3)

u∧ T ≡ −u∧
u∧ v ≡ −v ∧ u
u∧ u ≡ 0
(W u)∧ ≡ u∧ (tr(W )1 − W ) − W T u∧
u∧ v ∧ ≡ −(uT v)1 + vuT
u∧ W v ∧ ≡ −(−tr(vuT )1 + vuT )
×(−tr(W )1 + W T ) + tr(W T vuT )1 − W T vuT
u∧ v ∧ u∧ ≡ u∧ u∧ v ∧ + v ∧ u∧ u∧ + (uT u)v ∧
(u∧ )3 + (uT u)u∧ ≡ 0
u∧ v ∧ v ∧ − v ∧ v ∧ u∧ ≡ (v ∧ u∧ v)∧
[u∧ , v ∧ ] ≡ u∧ v ∧ − v ∧ u∧ ≡ (u∧ v)∧
[u∧ , [u∧ , . . . [u∧ , v ∧ ] . . . ]] ≡ ((u∧ )n v)∧
|
{z
}



u2
−u1 
0

(αu + βv)∧ ≡ αu∧ + βv ∧

李代数
 ∧ 
0
u1
u∧ = u2  =  u3
−u2
u3

表 1: SO(3) 性质与其近似形式

7

cos ϕ
ϕ−4 cos ϕ
≡ 1 + 3 sin ϕ−ϕ
ξ ⋏ + 4−ϕ sin2ϕ
(ξ ⋏ )2
2
2ϕ )
(
)
(
3
2−ϕ sin ϕ−2 cos ϕ
sin ϕ−ϕ cos ϕ
⋏
(ξ ) +
(ξ ⋏ )4
+
2ϕ3
2ϕ4
⋏
≈ 1 +[ ξ
]
C (J ρ)∧ C
T = Ad (T ) ≡
0
C
tr (T ) ≡ 2 cos ϕ + 2, det (T ) ≡ 1
Ad (T1 T2 ) =
2)
∑Ad (T11 ) Ad (T
∧ n
∧
T −1 ≡ exp (−ξ ∧ ) ≡[ ∞
n=1 n! (−ξ] ) ≈ 1 − ξ
T
T
C
−C r
T −1 ≡
0T
1
∑
1
⋏ n
⋏
T −1 ≡ exp (−ξ ⋏ )[≡ ∞
n=1 n! (−ξ ) ] ≈ 1 − ξ
C T −C T (J ρ)∧
−1
T
≡
0
CT
Tξ≡ξ
T ξ∧ ≡ ξ∧ T , T ξ⋏ ≡ ξ⋏ T
∧
⋏
⋏ −1
(T x) ≡(T x∧ T −1
) , (T x) ≡ T x T
exp( (T x)∧) ≡ T exp (x∧ ) T −1
exp (T x)⋏ ≡ T exp (x⋏ ) T −1
(T p)⊙ ≡ T p⊙ T −1
T
⊙T
(T p)
(T p)⊙ ≡ T −T p⊙ p⊙ T −1

李群
[ ]
ρ
ξ=
ϕ
∑
1
(ξ ∧ )n )
T = (exp (ξ ∧ ))≡ ∞
n=0 (
n!
1−cos ϕ
ϕ−sin ϕ
2
∧
∧
≡1+ξ +
(ξ ) +
(ξ ∧ )3
ϕ2
ϕ3
∧
≈ [1 + ξ
]
C
Jρ
T ≡ T
0
1
ξ ⋏ ≡ ad
(ξ ∧ )
∑
∞
1
⋏
⋏ n
( T = exp (ξ) ) ≡ (n=1 n! (ξ )
)
J =

∫1

J Q
J ≡
0 J
∑
Bn
⋏ n
J −1 ≡ ∞
− ]21 ξ ⋏
n=0
[ −1n! (ξ )−1≈ 1 −1
J
−J
QJ
J −1 ≡
0
J −1
∑∞ ∑∞
1
Q = n=0 m=0 (n+m+2)!
(ϕ∧ )n ρ∧ (ϕ∧ )m
(
)
ϕ
(ϕ∧ ρ∧ + ρ∧ ϕ∧ + ϕ∧ ρ∧ ϕ∧ )
≡ 1 ρ∧ + ϕ−sin
3
ϕ
)
( 22
ϕ +2 cos ϕ−2
(ϕ∧ ϕ∧ ρ∧ + ρ∧ ϕ∧ ϕ∧ − 3ϕ∧ ρ∧ ϕ∧ )
+
2ϕ4
(
)
2ϕ−3 sin ϕ+ϕ cos ϕ
+
(ϕ∧ ρ∧ ϕ∧ ϕ∧ + ϕ∧ ϕ∧ ρ∧ ϕ∧ )
(2ϕ5
)
(
)
∧
exp ( (ξ + δξ) ) ≈ exp ((J δξ)∧ ) exp (ξ ∧ )
⋏
⋏
exp (ξ + δξ)
≈ exp (J δξ) exp (ξ ⋏ )
T ≡ 1 + ξ⋏ J
J ξ⋏ ≡ ξ⋏ J
J (ξ) (≡ T J (−ξ)
)
(exp (δξ ∧ ) T )α ≈ 1 + (A (α, ξ) δξ)∧ T α
∑
Fn (α) ⋏ n
(ξ )
A (α, ξ) = αJ (αξ) J (ξ)−1 = ∞
n=0
n!

∑∞
1
α
(ξ ⋏ )n
n=0
0 T dα)≡
( (n+1)!
)
cos ϕ
ϕ−4 cos ϕ
ξ ⋏ + 4ϕ−5 sin2ϕϕ+ϕ
(ξ ⋏ )2
= 1 + 4−ϕ sin2ϕ
2
3
)
(
)
(
3
2ϕ−3
sin
ϕ+ϕ
cos
ϕ
2−ϕ sin ϕ−2 cos ϕ
(ξ ⋏ ) +
(ξ ⋏ )4
+
2ϕ4
2ϕ5
1 ⋏
≈1+
[ 2ξ ]
(

（左）雅可比

α, β ∈ 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

[x⋏ , [x⋏ , . . . [x⋏ , y ⋏ ] . . . ]] ≡ ((x⋏ )n y)⋏
|
{z
}
n
[ ]⊙ [
]
η1 −ε∧
ε
p⊙ =
= T
η
0
0T
[ ]⊚ [
]
ε
0
ε
p⊚ =
=
η
−ε∧ 0
x∧ p ≡ p⊙ x
pT x∧ ≡ xT p⊚

n

[ ]∧ [ ∧
]
v
u
u
x∧ =
= T
v
0
0
[ ]⋏ [ ∧
]
u
v
u∧
⋏
x =
=
∧
v
0
v
(αx + βy)∧ ≡ αx∧ + βy ∧
(αx + βy)⋏ ≡ αx⋏ + βy ⋏
x⋏ y ≡ −y ⋏ x
⋏
x
( x≡
) 0
(x∧ )4 + v T v (x∧ )2 ≡ 0
(
)
(
)2
(x⋏ )5 + 2 v T v (x⋏ )3 + v T v (x⋏ ) ≡ 0
∧
∧
∧
∧
∧
∧
[x , y ] ≡ x y − y x ≡ (x⋏ y)∧
[x⋏ , y ⋏ ] ≡ x⋏ y ⋏ − y ⋏ x⋏ ≡ (x⋏ y)⋏
[x∧ , [x∧ , . . . [x∧ , y ∧ ] . . . ]] ≡ ((x⋏ )n y)∧
{z
}
|

李代数

表 2: SE(3) 性质与其近似形式

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 个位姿，它的误
差可定义为：
−1
ei = ∥ log(Tgi
Tei )∨ ∥2 .

(7)

即两个位姿之差的李代数二范数。于是，可以定义两条轨迹的均方根（Root-Mean-Square-Error, RMSE）
误差为：

v
u n
u1 ∑
RMSE(g, e) = t
e2 .
n i=1 i

(8)

我为你准备了 code/ground-truth.txt 和 code/estimate.txt 两条轨迹。请你根据上面公式，实现 RMSE
的计算代码，给出最后的 RMSE 结果。作为验算，参考答案为：2.207。
注：
1. 公式 (7) 满足度量的定义：非负性、同一性、对称性、三角不等式，故形成距离函数。类似的，可
以定义 SO(3) 上的距离为：

(
)∨
d (R1 , R2 ) = ln R1−1 R2

.
2

关于距离定义可以参见拓扑学或者泛函教材。
2. 实际当中的轨迹比较还要更复杂一些。通常 ground-truth 由其他传感器记录（如 vicon），它的采
样频率通常高于相机的频率，所以在处理之前还需要按照时间戳对齐。另外，由于传感器坐标系不
一致，还需要计算两个坐标系之间的差异。这件事也可以用 ICP 解得，我们将在后面的课程中讲
到。
3. 你可以用上题的画图程序将两条轨迹画在同一个图里，看看它们相差多少。

9



---

Source Exif Data:

File Type                       : PDF
File Type Extension             : pdf
MIME Type                       : application/pdf
PDF Version                     : 1.5
Linearized                      : No
Page Mode                       : UseOutlines
Page Count                      : 9
Has XFA                         : No
Creator                         : Adobe Acrobat 11.0
Title                           : 第三节课习题
Author                          : 高翔
Producer                        : Acrobat Web Capture 11.0
Create Date                     : 2018:06:01 16:32:37+01:00
Modify Date                     : 2018:06:03 01:06:05+08:00

EXIF Metadata provided by EXIF.tools