转载地址:冯兵一个简单的视觉里程计实现(2)
前面讲述了特征提取,后期可以尝试orb_slam中采用orb,对特征加入方向信息,使得特征具备旋转不变性。
特征跟踪
特征提取是一个比较耗时的操作,即使使用了FAST特征检测,而且都是使用特征提取之后要进行特征匹配,通过RANSAC等方法进行特征提纯等操作,然后由于两帧之间时间间隔较短,完全可以采用跟踪的方法来预测下一帧的特征点。
我们采用KLT跟踪算法,具体算法的介绍可以参考原论文或者可以参考Learning OpenCV第十章的跟踪。
跟踪算法的好坏影响到特征点的对应关系,后续会对这一块进行重点研究。目前直接采用OpenCV中calcOpticalFlowPyrLK方法即金字塔的KLT算法,采用金字塔的KLT算法是为了解决运动范围过大及不连贯的情况,通过在图像金字塔的最高层计算光流,用得到的运动估计结果作为下一次金字塔的起始点,重复这个过程直到金字塔的最底层。具体将图像
It
It中检测到的特征
Ft
跟踪到图像
It+1
中,得到对应的特征
Ft+1
。具体调用如下:
1 | void VisualOdometry::featureTracking(cv::Mat image_ref, cv::Mat image_cur, |
这边记录了跟踪之后对应特征之间的像素距离存入std::vector<double>& disparities
中,后续我们会对这个像素距离设定阈值,以确保初始位置确定的准确度。
在我们对特征进行跟踪的时候,跟踪到的特征会越来越少(由于有相同视野的部分越来越小),因此设定一个阈值以保证特征的个数,小于给定阈值的时候,我们重新进行特征检测。
本质矩阵估计
这一块的公式乱码了,还是原文博主网站清晰,好看
本质矩阵是归一化图像坐标下的基础矩阵的特殊形式,归一化图像坐标也就是用当前像素坐标左乘相机矩阵的逆。具体表示:
已知摄像机矩阵
P=K[R|t]
P=K[R|t],令
x
x为图像上一点,
X
X为该点对应的世界坐标,则
x=PX
x=PX,如果已经知道相机内参矩阵
K
K,则令
xˆ=K−1x
x^=K−1x,即
xˆ=[R|t]X
x^=[R|t]X为归一化图像坐标。则两幅图像的对应点
x←→x′
x←→x′对应的归一化图像坐标与本质矩阵
E
E满足如下定义
xˆ′TExˆ=0
x^′TEx^=0.
具体可以通过5点法进行求解,更多的解法可以参考http://cmp.felk.cvut.cz/minimal/5_pt_relative.php,本文主要通过OpenCV3.0中cv::findEssentialMat
方法进行计算,主要思想采用随机抽样一致性算法(RANSAC),由于所有的点不是完全的匹配,如果用最小二乘,难免因为很多误匹配的点导致最后的结果有很大的偏差,RANSAC的做法,随机抽取5个点用于估计两帧之间的本质矩阵,检测其它点是否满足该本质矩阵,得到满足该本质矩阵的个数(称为内点),然后进行多次迭代,得到内点最多的那组计算出的本质矩阵即为所求解。更详细的过程仍可参考OpeCV中的代码实现,后续会自行设计与实现该过程。
由本质矩阵恢复相机矩阵
对于本质矩阵,我们知道其有另外一种定义方式即
E=[t]×R
E=[t]×R,
R
R表示旋转矩阵,
[t]×
[t]×表示
t
t的反对称矩阵,用于方便的进行叉乘运算。
当假设第一幅图像的摄像机矩阵
P=[I|0]
P=[I|0]的时候,对于本质矩阵通过SVD分解,可以计算出第二幅图像的摄像机矩阵。
具体在OpenCV3.0中可以通过调用cv::recoverPose
恢复相机相对旋转矩阵和平移。
确定相机轨迹
通过本质矩阵可以得到两帧计算的相对位置关系,因为第二幅图像对应相机矩阵是在假设第一幅图像的相机矩阵为
P=[I|0]
P=[I|0]的情况,因此我们对每次计算的相对量进行累加,设当前相机的旋转矩阵和平移为
Rcur,tcur
Rcur,tcur具体计算如下:
Rcur=R∗Rcur
Rcur=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的动态库,需自己编译,或者可以直接使用静态库,注意项目的配置。