简单快速的视觉里程计入门（Visual odometry）

视觉里程计（Visual Odometry）

这篇文章的初衷是希望给读者提供一个简单快速的入门视角。
作为一个robotics新手，看到高手们高屋建瓴的文章，难免不容易理解。
模型：用最简单的差速（两个轮子）机器人来举例。

基础知识

• 差速机器人小车的原理 （differential drive robot）
• 机器人学的坐标系概念以及表示（SE(3), pose，rotation matrix，etc）

里程计->视觉里程计

里程计（odometry）就是说相对于一个参考系，我怎么才能知道机器人离这个参考系走了多远？

举个栗子：车轮里程计（wheel odometry）是最原始的里程计。

原理：我们用轮编码器（wheel encoder）去记录机器人的两个轮子转动的情况，通过积分（一般用欧拉方法），我们就可以知道小车的位置和姿态，所以我们知道了小车的“里程”。

那么什么是视觉里程计（Visual odometry）呢？

目的：因为小车的轮编码器、车轮打滑等误差因素,我们记录小车的运动会随时间产生越来越大的误差（如下图）。

人们就想到很多用外部世界的特征点（feature points）去不断地告诉小车：“hi，小车，你在这里！” 。这就是视觉里程计（visual odometry）的思想。

视觉里程计的功能：
（1） 定位（localise）：通过周围环境的特征点估算自己的位置
（2） 测绘（local map）：估算外部世界特征点的位置

（* 没看懂这个？问题不大，看完下面就悟了）

视觉里程计问题的假设

1. 选一个参考坐标系{0} (referecne frame)和另一个固定在小车上的坐标系{B}（Body-fixed-frame，以下简称BFF）

2. 用SE(3) 描述BFF坐标系的位姿（位置+姿态）。
   需要知道的是，SE(3) 所包含的信息是：位置 + 旋转情况。
   (*SE， special Euclidean group, 欧几里得群； SO，special orthogonal group,特殊正交群；常见的机器人学坐标，感兴趣可以找资料了解一下~)

3. 假设环境有 n 个静止不动的特征点（p1,p2,…,pn），这些点可以被小车的传感器（e.g. 一个相机）观察到；用y(i) 去描述被小车观察到的点。

1. 描述运动环境的特征点（从参考系{0}观察）


这里，用齐次坐标（homogeneous coordinates）去描述点的位置，
（最下面的1的意思是这个向量不是自由向量，如果最下面是0，则表示是自由向量，就是localised vector 和free vector的区别，比如：旋转矩阵就是自由向量；而速度、位置等信息就不是自由的哦)

所以，我们把这 n 个特征点表示成下面的形式：

2.机器人的姿态（robot pose）
Pose（姿态），包含机器人坐标系BFF的 位置Xp 和 旋转Rp 的信息 ，相对于参考系{0}

表示在SE（3） 中，注意，它其实是个4*4的矩阵。。。

在t时刻，机器人的姿态：

3. 特征点状态空间和测量
特征点的状态空间是固定的，从参考系{0}观察而来；

测量特征点是从机器人身上的BFF坐标系测量的，表示如下：

下面两张图感受一下，一个是从{0}测得特征点，另一个是从BFF来测量的特征点。

4.运动学公式

4.1 对于静止的特征点：

4.2 对于姿态：


Overall，特征点和姿态的运动学公式：

5.Visual odometry问题

Ohhhh~ ,终于到我们的主角了——VO

视觉里程计（Visual odometry）的问题就是找到一个估算值（phat，Phat）来最小化误差，就是让下面橙色线段最短！（机器人不断地在估算出 p hat 和 P hat的值）
这里有三种方法可以解决这个问题：

• 批量优化数据
• 随机滤波（stochastic filter）
• 确定性观察者

6.什么是观察者

观察者（observer），其实是一组微分方程

使得这组微分方程满足：

其中 Tn（3）：

7.VO算法(先给个结论)

需要测量的值：u是小车速度，q是角速度，yi是从小车那观察到的特征点位置

误差：


Final implementation algorithm：