文章目录
  1. 1. 特征跟踪
  2. 2. 本质矩阵估计
  3. 3. 由本质矩阵恢复相机矩阵
  4. 4. 确定相机轨迹
  5. 5. 分析

前面讲述了特征提取,后期可以尝试orb_slam中采用orb,对特征加入方向信息,使得特征具备旋转不变性。

特征跟踪

特征提取是一个比较耗时的操作,即使使用了FAST特征检测,而且都是使用特征提取之后要进行特征匹配,通过RANSAC等方法进行特征提纯等操作,然后由于两帧之间时间间隔较短,完全可以采用跟踪的方法来预测下一帧的特征点。
我们采用KTL跟踪算法,具体算法的介绍可以参考原论文或者可以参考Learning OpenCV第十章的跟踪。
跟踪算法的好坏影响到特征点的对应关系,后续会对这一块进行重点研究。目前直接采用OpenCV中calcOpticalFlowPyrLK方法即金字塔的KLT算法,采用金字塔的KLT算法是为了解决运动范围过大及不连贯的情况,通过在图像金字塔的最高层计算光流,用得到的运动估计结果作为下一次金字塔的起始点,重复这个过程直到金字塔的最底层。具体将图像$I_t$中检测到的特征$F_t$跟踪到图像$I_{t+1}$中,得到对应的特征$F_{t+1}$。具体调用如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
void VisualOdometry::featureTracking(cv::Mat image_ref, cv::Mat image_cur,
std::vector<cv::Point2f>& px_ref, std::vector<cv::Point2f>& px_cur, std::vector<double>& disparities)
{
const double klt_win_size = 21.0;
const int klt_max_iter = 30;
const double klt_eps = 0.001;
std::vector<uchar> status;
std::vector<float> error;
std::vector<float> min_eig_vec;
cv::TermCriteria termcrit(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, klt_max_iter, klt_eps);
cv::calcOpticalFlowPyrLK(image_ref, image_cur,
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, 0);

std::vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
std::vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
disparities.clear(); disparities.reserve(px_cur.size());
for (size_t i = 0; px_ref_it != px_ref.end(); ++i)
{
if (!status[i])
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
continue;
}
disparities.push_back(norm(cv::Point2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y)));
++px_ref_it;
++px_cur_it;
}
}

这边记录了跟踪之后对应特征之间的像素距离存入std::vector<double>& disparities中,后续我们会对这个像素距离设定阈值,以确保初始位置确定的准确度。
在我们对特征进行跟踪的时候,跟踪到的特征会越来越少(由于有相同视野的部分越来越小),因此设定一个阈值以保证特征的个数,小于给定阈值的时候,我们重新进行特征检测。

本质矩阵估计

本质矩阵是归一化图像坐标下的基础矩阵的特殊形式,归一化图像坐标也就是用当前像素坐标左乘相机矩阵的逆。具体表示:
已知摄像机矩阵$P=K[R|t]$,令$x$为图像上一点,$X$为该点对应的世界坐标,则$x=PX$,如果已经知道相机内参矩阵$K$,则令$\widehat x=K^{-1}x$,即$\widehat x=[R|t]X$为归一化图像坐标。则两幅图像的对应点$x\leftarrow\rightarrow x’$对应的归一化图像坐标与本质矩阵$E$满足如下定义${\widehat x’}^TE\widehat x = 0$.
具体可以通过5点法进行求解,更多的解法可以参考http://cmp.felk.cvut.cz/minimal/5_pt_relative.php,本文主要通过OpenCV3.0中cv::findEssentialMat方法进行计算,主要思想采用随机抽样一致性算法(RANSAC),由于所有的点不是完全的匹配,如果用最小二乘,难免因为很多误匹配的点导致最后的结果有很大的偏差,RANSAC的做法,随机抽取5个点用于估计两帧之间的本质矩阵,检测其它点是否满足该本质矩阵,得到满足该本质矩阵的个数(称为内点),然后进行多次迭代,得到内点最多的那组计算出的本质矩阵即为所求解。更详细的过程仍可参考OpeCV中的代码实现,后续会自行设计与实现该过程。

由本质矩阵恢复相机矩阵

对于本质矩阵,我们知道其有另外一种定义方式即$E=[t]_\times R$,$R$表示旋转矩阵,$[t]_\times$表示$t$的反对称矩阵,用于方便的进行叉乘运算。
当假设第一幅图像的摄像机矩阵$P=[I|0]$的时候,对于本质矩阵通过SVD分解,可以计算出第二幅图像的摄像机矩阵。
具体在OpenCV3.0中可以通过调用cv::recoverPose恢复相机相对旋转矩阵和平移。

确定相机轨迹

通过本质矩阵可以得到两帧计算的相对位置关系,因为第二幅图像对应相机矩阵是在假设第一幅图像的相机矩阵为$P=[I|0]$的情况,因此我们对每次计算的相对量进行累加,设当前相机的旋转矩阵和平移为$R_{cur},t_{cur}$具体计算如下:
$R_{cur} = R*R_{cur}$
$t_{cur} = t_{cur}+scale*(R_{cur}*t)$
注意:由于单目相机定位存在scale的问题,这边先采用真实数据计算两帧之间的距离作为scale,后续会对scale的问题进行进一步分析。
这样一个基本的视觉里程计就完成了,具体的效果图如下:

分析

最后将采用视觉里程计计算的结果与真实轨迹进行了对比,通过matlab将结果画出,具体效果图如下:

根据结果我们就会发现算法后期的误差是越来越大,而这个累计误差在目前是无法避免的,因为后续相机的位姿都依赖于前面相机的位姿,因此要保证更好的精度不仅仅考虑两帧的信息,而是应该考虑全部信息,而对于帧率本机测试大概在10帧每秒,还没有达到实时,如果考虑更多的帧的信息,效率肯定会进一步下降,怎么更好的提高效率也是下一步考虑的问题。
整个项目代码可参考:
https://github.com/yueying/LearningVO.git
目前项目依托OpenCV3.0,目前官方没有提供OpenCV3.0的动态库,需自己编译,或者可以直接使用静态库,注意项目的配置。

本文主要参考http://avisingh599.github.io/vision/monocular-vo/

文章目录
  1. 1. 特征跟踪
  2. 2. 本质矩阵估计
  3. 3. 由本质矩阵恢复相机矩阵
  4. 4. 确定相机轨迹
  5. 5. 分析