一种基于卷帘相机的高频视觉里程计方法

本发明涉及计算机视觉，尤其涉及一种基于卷帘相机的高频视觉里程计方法。

背景技术：

1、同时定位与地图构建(simultaneous localization and mapping，slam)是指搭载特定传感器的机器人，在一个未知环境中运动时构建环境地图，并同时估计其自身的运动。选用相机作为传感器的视觉slam(vslam)，由于其成本较低、图像分辨率较高并且可以构建较为稠密的地图，在增强现实/虚拟现实(ar/vr)、三维重建、自动驾驶、路径规划、智能机器人、无人机等领域中具有重要作用。大多数相机分为全局相机和卷帘相机，卷帘相机因为其成本较低被广泛用于消费级电子产品中，不同于全局相机同时曝光所有像素，卷帘相机的像素采用逐行曝光的方式，每一行图像都具有不同的位姿。目前主流的vslam算法及视觉里程计(vo)方法都是基于全局曝光实现的，在使用卷帘相机成像过程中忽略了卷帘效应，这会导致系统跟踪的精度下降，尤其在快速运动时跟踪极易丢失。

2、在机器人快速运动的情况下，例如头戴ar/vr设备的用户，可以快速行走并快速旋转头部，要精准匹配用户头部运动所需跟踪频率远高于相机的帧率。一般的视觉里程计常结合惯性测量单元(imu)来估计相机的位置和旋转，但会带来不同传感器之间的校准问题，成本也会相应增加。使用全局相机进行跟踪的设备，由于全局相机存在机械极限的最小曝光时间，无法实现极快的跟踪。而卷帘相机具有更小的曝光时间，可以达到更高的帧率，且成本更低，相机高速旋转运动的情况下要实现高频高质量跟踪，保持低延迟目标不易丢失，在静态且光线充足的环境下，卷帘相机是最佳且最具成本效益的选择。因此，研究静态场景下基于卷帘相机的高频视觉里程计，提高视觉里程计在高速运动时的精度并降低端到端的系统延迟具有研究意义和实用价值。

技术实现思路

1、本发明的目的在于针对现有技术中高速运动的情况下相机跟踪精度下降甚至丢失、跟踪频率低、系统延迟高的问题，提供一种基于卷帘相机的高频视觉里程计方法。本发明适用于在快速运动时尤其高速旋转情况下，获得准确、低延迟的跟踪，大幅度提高了系统跟踪的频率和定位的精度。

2、本发明的目的是通过以下技术方案来实现的：一种基于卷帘相机的高频视觉里程计方法，包括以下步骤：

3、s1、通过卷帘相机组获取当前环境不同视角的卷帘畸变图像，并同步获取同一时刻卷帘相机组同时曝光的图像行索引；

4、s2、在当前卷帘畸变图像行上获取跟踪点，对卷帘畸变图像去畸变，通过旋转补偿重建上一帧图像行块，在重建图像行上寻找最佳匹配点；

5、s3、根据步骤s2得到各卷帘相机同时曝光的行图像上匹配好的最佳匹配点，通过对极约束，估计初始卷帘相机组图像行位姿；

6、s4、在关键帧上对每行特征点进行深度滤波，新一帧的卷帘图像传入，利用帧间行图像位姿计算当前帧上极线方程的参数矩阵，通过最小二乘优化方法构建卷帘对极曲线，搜索以获得特征点在当前帧的最佳匹配点位置；

7、s5、根据特征点及其最佳匹配点所满足的对极几何约束使用三角测量方法恢复特征点单次测量的深度和不确定性；

8、s6、对相机特征点上一状态最优深度和当前测量的深度融合，待重建行上的特征点深度收敛后将特征点加入地图，最小化重投影误差得到优化卷帘相机组图像行位姿。

9、进一步地，所述卷帘相机组包括多个卷帘相机，多个所述卷帘相机放置在不同方向上，且每个所述卷帘相机都与其他至少一个卷帘相机有重叠视野，所述卷帘相机之间在x、y、z三个方向上都有旋转和平移，且偶数卷帘相机在图像平面上旋转90°。

10、进一步地，所述获取同一时刻卷帘相机组同时曝光的图像行索引，具体包括：为每个卷帘相机安装一个以固定频率闪烁的led灯，且灯光占据卷帘相机的整个视野，比较前后两帧卷帘相机行图像的平均灰度值的差值，若该差值超过预设的光度阈值则，表示led灯闪烁时当前帧当前行正在被曝光，返回当前行的图像行索引，即为同一时刻卷帘相机组同时曝光的图像行索引。

11、进一步地，所述步骤s2具体包括：

12、计算卷帘畸变图像行的灰度梯度，将梯度变化最大的前多个点作为行图像的跟踪点；

13、在相机坐标系下，使用布朗径向畸变模型的前两个参数k1和k2，对径向畸变的行图像进行建模，将归一化平面上的畸变点pcdistorted＝[xd,yd,1]t进行去畸变，得到原始点pc_undistorted＝[xu,yu,1]t；

14、每传入一行新图像rowt，对行图像上的跟踪点进行畸变矫正，卷帘图像同一行具有相同的yd，但不同的xd对应空间中不同的yu位置，即卷帘畸变图像行经过去畸变，还原到原始图像上为一条曲线；

15、从前一帧中提取行图像rowi，其旋转为ri，先利用单应矩阵对相机图像进行旋转补偿，再去畸变重建行图像，其中，对于传入的图像rowt指定参考行rowj，以其旋转rj来代替当前行图像的旋转，当rowt与rowj之间的旋转超过设置的旋转阈值时，将当前行更新为新的参考行；

16、根据给定的相机内参矩阵k，确定行图像rowj和行图像rowi之间的单应变换关系，其表达式为hj,i＝krjritk-1，并基于该表达式对上一帧图像上的像素行进行单应变换，利用当前行图像的跟踪点在上一帧图像中进行块匹配：在跟踪点两侧各取长度为l的图像块a，然后在上一帧图像的重建行上灰度梯度最大的前n个点两侧也取同样长度的图像块bi,i＝1,2,...,n，并计算两个图像块的相似度，选取相似度最大的两个图像块作为匹配图像块，得到最佳匹配点。

17、进一步地，所述布朗径向畸变模型的表达式为：

18、xd＝xu(1+k1r2+k2r4)

19、yd＝yu(1+k1r2+k2r4)

20、

21、其中，xd表示畸变点在x方向上的坐标，yd表示畸变点在y方向上的坐标，xu表示原始点在x方向上的坐标，yu表示原始点在y方向上的坐标，r表示该原始点到图像中心的距离；

22、所述两个图像块的相似度的计算公式为：

23、

24、其中，s(a,b)ncc表示图像块a和图像块b的相似度，a(i,j)表示图像块a的灰度矩阵中第i行第j列的像素点灰度值，表示图像块a的灰度均值，b(i,j)表示图像块b的灰度矩阵中第i行第j列的像素点灰度值，表示图像块b的灰度均值。

25、进一步地，所述步骤s3具体包括：

26、令nξcam∈se(3)表示卷帘相机n相对于卷帘相机组中心坐标系的相对位姿，即卷帘相机组中心到卷帘相机n的相对位姿，固定不变，通过外参校准获取；

27、设3d世界点pw＝[xw,yw,zw,1]t，在t1时刻和t2时刻在卷帘相机n坐标系下的坐标分别为p(n,t1)＝[x1,y1,z1,1]t，p(n,t2)＝[x2,y2,z2,1]t，有：

28、p(n,t1)＝nξcamcamξw(t1)pw

29、

30、其中，camξw表示卷帘相机组中心相对于世界坐标系的相对位姿，代替camξw(t2)camξw-1(t1)，表示从t1时刻到t2时刻卷帘相机组的位姿变化，camξw(t1)表示t1时刻卷帘相机组相对于世界坐标系的位姿，camξw(t2)表示t2时刻卷帘相机组相对于世界坐标系的位姿，camξn表示卷帘相机组相对于卷帘相机n的相对位姿，nξcam表示卷帘相机n相对于卷帘相机组的相对位姿；

31、3d点p(n,t1)和p(n,t2)的归一化坐标分别为pc(n,t1)＝[x1/z1,y1/z1,1]t，pc(n,t2)＝[x2/z2,y2/z2,1]t，通过相机内参矩阵kn投影到像素点puv(n,t1)和puv(n,t2)的像素位置分别为：

32、puv(n,t1)＝knpc(n,t1)

33、puv(n,t2)＝knpc(n,t2)

34、代入同一时刻各卷帘相机当前行图像的跟踪点和其最佳匹配点的像素坐标，得到归一化坐标，根据对极约束获取本质矩阵e，根据e＝s^r分解出旋转矩阵r和平移向量s，以获取卷帘相机组在t1时刻和t2时刻的相对位姿根据该相对位姿和初始位姿迭代相乘后获取卷帘相机组图像行位姿。

35、进一步地，所述步骤s4具体包括：

36、带有径向畸变的卷帘图像，对行扫描线去畸变生成无畸变图像中的一行曲线，卷帘对极曲线是由去畸变图像中的每一行曲线行与图像的极线的交点组成；定义无畸变像素点为uundistorted＝[uu,vu,1]t和畸变像素点为udistorted＝[ud,vd,1]t，其中uu、vu分别表示无畸变像素点在像素平面上的u轴和v轴坐标，ud、vd分别表示畸变像素点在像素平面上的u轴和v轴坐标，相机内参矩阵为无畸变像素点、畸变像素点和相机内参矩阵有以下关系：

37、uundistorted＝kpc_undistorted

38、udistorted＝kpc_distorted

39、其中，pc_undistorted＝[xu,yu,1]t和pc_distorted＝[xd,yd,1]t表示相机坐标系下归一化平面上的无畸变点和畸变点，xu、yu分别表示无畸变点在相机坐标系下归一化平面上的x轴和y轴坐标，xd、yd分别表示畸变点在相机坐标系下归一化平面上的x轴和y轴坐标，fx、fy分别代表图像平面上x轴和y轴的焦距，cx、cy分别代表了图像平面上主点的坐标；

40、根据上述对径向畸变图像建模步骤，通过下列最小二乘优化方法寻找去畸变图像曲线行与极线的交点：若已知极线方程为au+bv+c＝0，其中a、b、c为极线方程参数；

41、当a≠0时，对于畸变图像行yd，求解目标函数：

42、

43、其中，xu＝[(-b/a)vu-(c/a)-cx]/fx，yu＝(vu-cy)/fy，求得去畸变图像曲线行与极线交点为u*＝[uu*,vu*,1]t，其中uu*＝-(b/a)vu*-(c/a)，r为无畸变点到图像中心的距离，k1、k2为畸变参数；

44、当a＝0时，即极线水平时，求解目标函数：

45、

46、其中，xu＝(uu-cx)/fx，yu＝(-c/b-cy)/fy，可求得交点u*＝[uu*,vu*,1]t，其中vu*＝-c/b，当前情况求得两个交点，根据先验深度计算特征点与所得交点之间的重投影误差，剔除非目标交点；

47、取关键帧上每一行的跟踪点为要进行深度滤波的特征点，对于一个特征点p，所在行图像为rowp，根据步骤s3中得到的已知帧间行图像相对位姿，获取当前帧上极线方程的参数矩阵[a,b,c]t，由此在当前帧上得到一条卷帘对极曲线，特征点的投影点在卷帘对极曲线上搜索，通过最小光度误差的比对算法获得最佳匹配点位置q。

48、进一步地，所述步骤s5具体包括：

49、设图像上像素点的逆深度ρ＝z-1满足高斯分布模型：

50、p(ρ)＝n(m,τ2)

51、其中，z表示深度，m为逆深度均值，τ2为逆深度不确定性；

52、已知关键帧上的一个特征点p投影到普通帧上投影点为q，两点所在行图像的相对位姿qξp，通过三角测量方法恢复特征点的单次测量深度估计；

53、通过假设特征点在卷帘对极曲线上存在一个像素的偏差，计算深度估计的不确定性。

54、进一步地，所述步骤s6具体包括：

55、相机特征点经过上一次融合后逆深度均值和逆深度不确定性分别为m和τ2；

56、当新图像到来对特征点做一次新的三角测量，得到当前状态对应的新的逆深度观测值，同样满足高斯分布：

57、

58、将当前状态对应的新的逆深度观测值与上一状态融合的逆深度进行深度融合，融合后的逆深度分布为有：

59、

60、

61、当逆深度不确定性小于预设的逆深度不确定性阈值时，则表示深度收敛，停止融合计算；否则，若深度未收敛则继续进行深度融合更新深度；

62、将深度收敛的特征点加入地图，最小化重投影误差，得到优化卷帘相机组图像行位姿。

63、与现有技术相比，本发明的有益效果是：本发明利用卷帘相机逐行曝光的特性，设置卷帘相机组，逐行对相机组的特征点进行高频跟踪；利用卷帘对极曲线实现像素点的搜索匹配，通过三角化和深度融合策略对特征点进行深度滤波；本发明有效提高了视觉里程计的跟踪频率，减小了系统延迟，使得机器人在快速运动尤其快速旋转情况下实现准确的定位；本发明在确保精度相近的情况下，极大地减小了计算量，从而达到实时性。