视觉里程计

最近读到了一个很好的blog，讲的简单易懂。里程计就是车里面常见的记录里程的仪表，通过车轮转过的圈数统计汽车走过的距离。视觉里程计主要指机器人的6DOF的轨迹。

我们可以用单目或者双目相机来实现视觉里程计（VO）。双目相机的好处是可以得到准确的轨迹，比如x方向动了1米，而单目相机只能得到一个不能确定尺度的轨迹，比如x方向动了1个单位。不过如果机器人很小，双目相机的baseline就会很小，对于远处的物体，双目相机实际上相当于一个单目相机了。因为现在的无人机正在变得越来越小，所以Scaramuzza组主要在研究单目相机。

双目VO

对于双目相机，我们的输入是在t和t+1时刻的图像，输出是这两个时刻间机器人旋转和平移的矩阵。具体来说，我们先计算disparity map，然后用FAST算法检测特征点，用KLT进行匹配。为了使特征均匀分布，我们在每个grid中都选取20个特征点。

在计算disparity map之前，我们要先进行一些预处理。首先要做undistortion，用来处理相机lens的distortion。然后是rectification，使得epipolar line变得平行，这样disparity map就只需要在一个方向做block matching。

在disparity map里我们可以得到这些特征点的3D位置，这样就有了两个点云。接下来，我们需要得到这些点云在真实3D世界中的坐标。在匹配中选取哪些互相配对的匹配点，然后在这些inliers中计算运动矩阵。这里的inlier是通过maximum clique problem找到的，就是寻找一个graph中的subset，在这个subset里面所有node都是互相连通的。

我们可以用Levenberg Marquardt方法来求最小值，这里的目标函数就是特征点在t和t+1时刻图像的匹配度，还有它们的3D坐标在运动轨迹中的匹配度。

一个重要的hack是当一个很大的车占据图像的大部分的时候，scene rigidity的假设不再成立。这时我们只保留那些主要运动方向是向前的运动矩阵。这个hack会极大的提高算法在KITTI上的performance。

单目VO

和双目一样，单目VO的input也是两个时刻的图像，输出是R和T。只不过这里的T不考虑尺度。首先我们还是undistort这些图像，然后用FAST检测特征点。接下来用Nister的5点算法和RANSAC来计算essential matrix。在essential matrix中得到R和t。通过其他sensor得到尺度信息来更正R和T。

在OpenCV中，我们可以用FAST函数来检测特征点。然后用KeyPoint::convert来把KeyPoint类型转换成Point2f类型，用来做特征跟踪。KLT的函数是featureTracking。在KLT中，会有一些点被丢掉。当总特征点数很少时，我们需要重新检测特征点。通过匹配的特征点，可以得到essential matrix。因为essential matrix只有5个自由度，我们可以用5点算法来计算它。

如果特征匹配是完美的，我们只需要5个匹配就可以了。但实际上肯定有误匹配。所以需要RANSAC来筛选inlier。在每个iteration，我们随机选取5个匹配，然后计算essential matrix。接着查看其它点是否满足这个essential matrix，直到得到了足够多的支持点。在OpenCV中这一切都可以通过findEssentialMat实现。得到essential matrix之后，就可以计算pose。这个函数是recoverPose。相机的轨迹就是R_pose = RR_pose，T_pose = T_pose + TR_pose。

和双目VO一样，这里也假设几乎所有点都是rigid。但是我们可能会遇到一辆巴士驶过的情况。算法可能会错误的认为运动是朝向侧面的。所以如果T不是向前的运动，我们就忽略它。