视觉SLAM理论与实践第三节课习题
第三节课习题
深蓝学院SLAM课程团队 2018 年 3 ⽉ 5 ⽇
1 习题说明 • 第 i 节课习题所有材料打包在 Li.zip 中，∀i = 1…8。 • 习题分为若⼲种：计算类习题，需要读者编程计算⼀个实际问题，我们会附有参考答案以供⾃测。 操作类习题，会指导读者做⼀个具体的实验，给出中间步骤截图或结果。简述类习题则提供阅读材 料，需要读者阅读材料后，回答若⼲问题。 • 每个习题会有⼀定的分值。每次习题分值加和为 10 分。你需要获得 8 分以上才能得到“通过”的评 价。带 ∗ 的习题为附加题，会在总分之外再提供⼀定的分值，所以总和可能超过 10 分。换句话说， 你也可以选择⼀道附加题，跳过⼀道正常题。 • 每道习题的给分由助教评判，简述类习题可能存在⼀定开放性，所以评分也存在主观因素。 • 请利⽤深蓝学院系统提交习题。每次习题我们会记通过与否。提交形式为 word 或 pdf 格式报告， 如有编程习题请提交可编译的源码。 • 为⽅便读者，我通常会准备⼀些阅读材料，放在 books/或 papers/⽬录下。请读者按个⼈需求使⽤ 这些材料。它们多数是从⽹络下载的，如果侵犯到你的权利，请及时告诉我。 • 每个习题会标注⼤致⽤时，但视同学个⼈⽔平可能会有出⼊。 • 习题的完成情况会影响你对本课程内容的掌握程度，请认真、独⽴完成。习题总得分较高的同学将 获得推荐资格。
2 群的性质 (2 分，约 1 小时)
课上我们讲解了什么是群。请根据群定义，求解以下问题： 1. {Z,+} 是否为群？若是，验证其满⾜群定义；若不是，说明理由。 2. {N,+} 是否为群？若是，验证其满⾜群定义；若不是，说明理由。 其中 Z 为整数集，N 为⾃然数集。
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,×) 构成李代数
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ρ 0T 1 . (2) 令 ρ = θa，那么：
∞ ∑ n=0
1 (n + 1)!
(ϕ∧)n = sinθ θ
I +(1− sinθ θ )aaT + 1−cosθ θ
a∧ ∆ = J. (3)
这也正是课件⾥提到的左雅可⽐。 提⽰：类⽐于 SO(3) 的泰勒展开，然后合并奇偶数项级数即得。
5 伴随 (2 分，约 1 小时)
在 SO(3) 和 SE(3) 上，有⼀个东西称为伴随（Adjoint）。下⾯请你证明 SO(3) 伴随的性质。 对于 SO(3)，有： Rexp(p∧)RT = exp((Rp)∧). (4)此时称 Ad® = 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)ξ)∧) (5)其中 Ad(T) 定义为： Ad(T) =[ R t∧R 0 R ]. (6) 这个性质将在后⽂的 Pose Graph 优化中⽤到。但是 SE(3) 的证明较为复杂，不作要求。 完整的 SO(3) 和 SE(3) 性质见1和2。
6 轨迹的描绘 (2 分，约 1 小时)
我们通常会记录机器⼈的运动轨迹，来观察它的运动是否符合预期。⼤部分数据集都会提供标准轨迹 以供参考，如 kitti、TUM-RGBD 等。这些⽂件会有各⾃的格式，但⾸先你要理解它的内容。记世界坐标 系为 W，机器⼈坐标系为 C，那么机器⼈的运动可以⽤ TWC 或 TCW 来描述。现在，我们希望画出机器 ⼈在世界当中的运动轨迹，请回答以下问题： 1. 事实上，TWC 的平移部分即构成了机器⼈的轨迹。它的物理意义是什么？为何画出 TWC 的平移 部分就得到了机器⼈的轨迹？ 2. 我为你准备了⼀个轨迹⽂件（code/trajectory.txt）。该⽂件的每⼀⾏由若⼲个数据组成，格式为
[t,tx,ty,tz,qx,qy,qz,qw],
其中 t 为时间，tx,ty,tz 为 TWC 的平移部分，qx,qy,qz,qw 是四元数表⽰的 TWC 的旋转部分，qw 为四元数实部。同时，我为你提供了画图程序 draw_trajectory.cpp ⽂件。该⽂件提供了画图部分 的代码，请你完成数据读取部分的代码，然后书写 CMakeLists.txt 以让此程序运⾏起来。注意我 们需要⽤到 Pangolin 库来画图，所以你需要事先安装 Pangolin（如果你做了第⼀次作业，那么现 在已经安装了）。CMakeLists.txt 可以参照 ORB-SLAM2 部分。
书上P90-91；现成的。
7 * 轨迹的误差 (2 分，约 1 小时)
本题为附加题。 除了画出真实轨迹以外，我们经常需要把 SLAM 估计的轨迹与真实轨迹相⽐较。下⾯说明⽐较的原 理，请你完成⽐较部分的代码实现。 设真实轨迹（ground-truth）为 Tg，估计轨迹 Te。它们都以 TWC 的形式存储，格式同上题。现在， 你需要计算估计轨迹的误差。我们假设每⼀个 Tg 都与给定的 Te 对应。那么，对于任意第 i 个位姿，它的 误差可定义为： ei = ∥log(T−1 gi Tei)∨∥2. (7) 即两个位姿之差的李代数⼆范数。于是，可以定义两条轨迹的均⽅根（Root-Mean-Square-Error, RMSE） 误差为： RMSE(g,e) =v u u t1 n n ∑ i=1 e2 i. (8) 我为你准备了 code/ground-truth.txt 和 code/estimate.txt 两条轨迹。请你根据上⾯公式，实现 RMSE 的计算代码，给出最后的 RMSE 结果。作为验算，参考答案为：2.207。 注： 1. 实际当中的轨迹⽐较还要更复杂⼀些。通常 ground-truth 由其他传感器记录（如 vicon），它的采 样频率通常⾼于相机的频率，所以在处理之前还需要按照时间戳对齐。另外，由于传感器坐标系不 ⼀致，还需要计算两个坐标系之间的差异。这件事也可以⽤ ICP 解得，我们将在后⾯的课程中讲 到。 2. 你可以⽤上题的画图程序将两条轨迹画在同⼀个图⾥，看看它们相差多少。
书上P90-91；现成的。