一个简单的视觉里程计实现(2)

转载地址：冯兵一个简单的视觉里程计实现(2)

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

特征跟踪

特征提取是一个比较耗时的操作，即使使用了FAST特征检测，而且都是使用特征提取之后要进行特征匹配，通过RANSAC等方法进行特征提纯等操作，然后由于两帧之间时间间隔较短，完全可以采用跟踪的方法来预测下一帧的特征点。
我们采用KLT跟踪算法,具体算法的介绍可以参考原论文或者可以参考Learning OpenCV第十章的跟踪。
跟踪算法的好坏影响到特征点的对应关系，后续会对这一块进行重点研究。目前直接采用OpenCV中calcOpticalFlowPyrLK方法即金字塔的KLT算法，采用金字塔的KLT算法是为了解决运动范围过大及不连贯的情况，通过在图像金字塔的最高层计算光流，用得到的运动估计结果作为下一次金字塔的起始点，重复这个过程直到金字塔的最底层。具体将图像ItIt中检测到的特征Ft跟踪到图像It+1中，得到对应的特征Ft+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& px_ref, std::vector& 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 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::iterator px_ref_it = px_ref.begin(); std::vector::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& disparities中，后续我们会对这个像素距离设定阈值，以确保初始位置确定的准确度。
在我们对特征进行跟踪的时候，跟踪到的特征会越来越少（由于有相同视野的部分越来越小），因此设定一个阈值以保证特征的个数，小于给定阈值的时候，我们重新进行特征检测。

本质矩阵估计

这一块的公式乱码了，还是原文博主网站清晰，好看

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

由本质矩阵恢复相机矩阵

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

确定相机轨迹

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

分析

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

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

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