一种基于仿生眼的双目主动视觉半稠密深度估计方法

本发明属于计算机视觉，涉及仿生眼运动控制、相机位姿与路标点坐标估计优化和半稠密深度估计问题，具体涉及一种基于仿生眼的双目主动视觉半稠密深度估计方法。

背景技术：

1、在机器人领域，移动机器人在未知环境中的自主导航能力一直是一个备受瞩目且至关重要的研究方向。slam技术为移动机器人在未知环境中定位自身位置并构建地图提供了关键解决方案。双目被动视觉和单目被动视觉slam在目前slam领域中有较多研究。然而，在有动态物体、纹理分布不均的室外及大场景场合中，传统被动视觉slam仍然存在诸多的限制，导致算法的精度和鲁棒性受到影响甚至导致算法的失效。其原因是相机无法主动选择拍摄的场景，从而无法避免纹理分布较低区域和动态物体对于slam算法的负面影响。

2、目前，主流的特征点法视觉slam仅对稀疏的特征点的三维位置进行估计，最终生成的地图仅包含这些稀疏的路标点。虽然这种稀疏地图足以满足机器人在地图中进行重定位的需求，但对于一些更高的需求，如导航、避障和场景三维重建等，其表现受到限制。因此，具备半稠密深度估计的能力对于提高地图的可用性、推动slam技术的实际应用至关重要。如今，视觉slam主要通过相机特性、深度学习或直接法进行半稠密或者稠密的深度估计，但目前在相关机器人上应用最多的还是使用rgb-d相机通过tof(time offlight)法或者结构光法等直接获取图像中所有像素的深度值，其原因是rgb-d相机使用较为容易且其可以直接通过传感器中硬件测量深度，无需消耗大量的计算资源来估计。然而，rgb-d相机的稠密建图只适用于一些特定的场景中，在室外或是大场景场合中，rgb-d相机还无法被很好地应用，且其功耗和成本较高。

技术实现思路

1、本发明的目的旨在提供一种基于仿生眼的双目主动视觉半稠密深度估计方法。

2、基于上述目的，本发明采取了如下技术方案：

3、一种基于仿生眼的双目主动视觉半稠密深度估计方法，包括以下步骤：

4、s100，利用双目仿生眼相机采集图像；

5、s200，将s100采集到的图像输入到slam系统中，利用slam系统追踪关键帧并估计相机位姿和路标点坐标；

6、s300，利用s200获取的关键帧位姿和其对应关键帧图像数据，通过极线搜索、块匹配结合三角测量，再利用深度滤波器实现半稠密的深度估计，具体是：

7、s301，选定进入步骤s300的一帧为参考关键帧，将参考关键帧所对应图像数据中所有像素深度所对应的逆深度假设为满足一个初始的高斯分布，为后续的深度信息融合提供一个初值；

8、s302，插入新的关键帧，通过极线搜索和块匹配确定投影点位置；

9、s303，根据参考关键帧和当前关键帧之间的几何关系计算三角化后的三维点深度及其不确定性；

10、s304，将当前观测深度所对应逆深度的高斯分布融合进上一次的估计中建立融合后逆深度的分布；若不确定性小于一定阈值，则认为深度数据收敛，确定三维点深度，否则返回步骤s302继续进行信息融合。

11、所述步骤s302具体是：

12、设参考关键帧所对应图像中的一个像素为u1，该像素所对应的深度范围在当前关键帧所对应图像中的投影为一条极线l2；将参考关键帧所对应图像记为ik，当前关键帧所对应图像记为it，世界坐标系相对于参考关键帧和当前关键帧的旋转矩阵分别为rkw和rtw，世界坐标系相对于参考关键帧和当前关键帧的平移向量分别为tkw和ttw，通过标定得左相机的内参kl，则参考关键帧和当前关键帧所对应图像在像素坐标系下的相互关系为：

13、

14、其中，uk为参考关键帧中的一个像素，dk为该像素所对应的空间点深度；当得知dk、uk时，就可以计算出ut的位置；当给uk的两个分量再增加一个增量du、dv时，可以算出ut的增量dut、dvt，由此得到局部范围内关键帧和当前帧图像坐标变换的一个线性关系构成的仿射变换：

15、

16、接着，根据仿射变换矩阵，将相机的旋转考虑进块匹配：

17、取u1周围的像素块为同时把l2上的像素块记为使用去均值的归一化互相关方式比较u1周围像素块和极线上像素块的相关性；当像素块梯度与极线所夹的夹角较大时，块匹配的可信度较低，因此此处只匹配像素梯度在极线方向或在极线方向一定阈值内的像素块，根据ncc的定义，有：

18、

19、确定步长后，通过遍历对应极线上规定像素块与源像素块的ncc值，可以得到一个非凸的ncc分布函数，寻找ncc最高的点作为源像素块的匹配像素块。

20、步骤s302中，若块匹配到的最高ncc值仍低于阈值，则认为此当前关键帧所对应图像中对于源像素块的极线搜索与块匹配失效，进入新的关键帧以及与之对应且经s200之slam系统优化后的相机位姿后，再进行匹配与信息融合收敛。

21、所述步骤s303中，通过三角测量对三维点深度进行计算，计算方式如下：

22、设有两帧图像分别为i1与i2，以i1作为参考，i2的变换矩阵为t；相机光心为o1与o2；在i1中有一个特征点p1，对应i2中有特征点p2，连接光心o1与特征点p1，光心o2与特征点p2，得到射线o1p1与o2p2；在相机不作纯平移运动时，直线o1p1与o2p2在场景中相交于一点p，该点是两个特征点p1与p2所对应的地图点在三维场景中的位置；

23、设x1与x2为特征点p1与p2所对应的归一化坐标，则其关系满足：

24、s2x2＝s1rx1+t，其中s1与s2分别为两个特征点p1与p2的深度，旋转矩阵r和平移向量t已知；

25、从几何角度，可以在射线o1p1上寻找三维点使其投影位置接近p2，同理也可以在射线o2p2上寻找；

26、以计算s1为例，变换上式得到只关于s1的方程：解方程直接求得s1；同理求得s2；此时初步确定两个特征点的深度；

27、因噪声影响o1p1与o2p2无法相交时，通过最小二乘法求解。

28、步骤s303中，当不断出现新的当前关键帧利用极线搜索和块匹配来找到对应像素块并利用三角测量来估计深度时，都会计算其在参考关键帧中的观测深度并转化为逆深度，此时的逆深度同样为一个高斯分布：

29、其中，通过极线搜索后像素的误差结合三角测量几何关系计算出的深度偏差而获得。

30、所述步骤s100中，采用全景相机对双目仿生眼相机进行注视引导，步骤包括：

31、s101，获取全景图像，然后检测全景图像中影响slam任务执行的关键场景信息，包括特征点以及动态物体；

32、s102，基于关键场景信息对全景图像中的像素点进行赋值，构建全景价值图像；

33、s103，将左右两个仿生眼相机的视场区域分别投影到全景价值图像中，得到左视场投影区域和右视场投影区域；同时控制左视场投影区域和右视场投影区域重叠；合并左视场投影区域和右视场投影区域，得到当前双目视场投影区域；

34、s104，在全景价值图像中得到当前双目视场投影区域的价值均值；判断当前双目视场投影区域的价值均值与价值阈值的大小：若当前双目视场投影区域的价值均值大于价值阈值，则不移动当前双目视场投影区域，并将左右两个仿生眼相机当前采集的高价值图像作为slam系统的输入；若当前双目视场投影区域的价值均值小于等于价值阈值，则在全景价值图像中搜索比价值阈值更高的目标双目视场投影区域，然后进入步骤s105；

35、s105，根据当前双目视场投影区域和目标双目视场投影区域，分别计算左右两个仿生眼相机所需的位移量；根据位移量分别移动左右两个仿生眼相机，最后将左右两个仿生眼相机在所述目标双目视场投影区域中采集的高价值图像作为slam系统的输入。

36、所述步骤s104中，

37、通过在全景图像可注视区域中采用与当前仿生眼相机注视区域投影大小相同的矩形作为滑动窗口，从当前注视区域以一定步长s由近致远进行搜索，再结合全景图像价值，得到每个窗口中的价值均值vm；

38、定义价值阈值vth，若当前注视区域的价值均值高于价值阈值，选择不移动相机；反之，若当前注视区域的价值均值小于等于价值阈值，选择将视线转移至价值均值高于价值阈值vth且与当前注视区域距离最近的区域。

39、所述步骤s105中，将左仿生眼相机的旋转运动表示为旋转矩阵rlclt，右相机的旋转运动表示为旋转矩阵rrcrt，通过如下方法求解rlclt：

40、将全景价值图中目标注视区域的中心像素坐标upt根据投影模型反投影至全景相机投影球面中，并将反投影至球面的投影点ppt转换至左相机初始位姿坐标系olo中获得左相机的目标光轴方向plot：将左仿生眼相机目前注视区域的像素平面中心点通过相机投影模型反投影至归一化平面，再将其转换为左仿生眼相机的初始位姿坐标系olo中获得左相机在初始位姿坐标系olo中的当前姿态ploc：由此可得，左仿生眼相机需要从当前姿态ploc移动到目标姿态plot；

41、计算当前姿态ploc与目标姿态plot之间旋转轴：

42、根据点乘的定义，对于旋转角度θlclt：

43、根据旋转角度和旋转轴，通过罗德里格斯公式可得旋转矩阵plclt：

44、

45、同理求解rrcrt；

46、通过求解左右相机的旋转矩阵，获得仿生眼控制模块需要对左、右仿生眼控制电机下达的旋转指令，使仿生眼相机将视域转移至场景中的高价值区域，实现注视引导。

47、所述slam系统采用orb-slam2框架改进的slam算法，包括四个模块，分别是追踪线程、局部建图线程、回环闭合线程和全局ba线程：

48、(1)通过将当前帧与局部地图进行特征点匹配，实现每一帧的跟踪，利用运动模式、pnp重定位模式或参考关键帧模式初始化相机位姿并追踪局部地图，采用pnp最小化重投影误差来估计相机的位姿，同时判断关键帧，作为追踪线程运行；

49、(2)管理局部地图并通过局部建图线程执行局部ba优化；

50、(3)使用dbow2库检测大型回环，并通过执行位姿图优化来校正相机轨迹累积误差，以此构成回环闭环线程；

51、(4)在完成位姿图优化后，启动全局ba线程，以获取全局一致的相机位姿和地图结构。

52、所述slam系统中，若当前帧满足下列任意一个条件，将当前帧设置为关键帧：

53、(1)当前帧为进行仿生眼视线控制到达目标注视区域后的第一帧；

54、(2)距离上一次全局重定位超过13帧；

55、(3)局部建图线程处于空闲状态；

56、(4)距离上一个设置的关键帧超过15帧；

57、(5)当前帧与上一个设置的关键帧之间的平移距离超过阈值tth；

58、(6)当前帧中追踪成功的特征点的数量达到70以上；

59、(7)当前帧中追踪成功的特征点数量少于参考关键帧的85％。

60、与现有技术相比，本发明具有如下有益效果：

61、(1)本发明基于人眼的视觉机制，当场景中的特征点分布不均时，能通过构建全景价值图将特征点相对密集的区域赋予高价值，从而引导仿生眼向此区域移动。密集的特征点有利于仿生眼相机的位姿估计优化和全局地图的构建，提高了slam系统的稳定性。同时也为后续仿生眼相机利用块匹配进行半稠密深度估计提供了纹理基础。

62、(2)本发明在构建全景价值图的过程中排除了动态物体对于相机状态估计优化和半稠密深度估计的干扰，若在半稠密深度估计的过程中出现了动态物体，则会影响块匹配过程，动态物体移动后，对应极线上原应匹配的像素点消失，进而匹配错误的像素块从而导致匹配失效或深度估计错误。因此，排除动态物体这一步骤为后续半稠密深度估计提供了质量更好的图像数据。

63、(3)本发明对于仿生眼相机相对位姿变换时的位姿估计和优化提供了一种改进后的优化算法，能够很好地通过四个线程来优化相机的位姿和左右仿生眼相机的相对位姿及路标点坐标，为后续半稠密深度估计步骤提供了基础。

64、(4)本发明利用了上述的优点进行了半稠密深度估计，排除了对深度估计过程的多数干扰项并提供了良好的数据。同时，为了更准确地进行深度估计，本发明对像素块梯度做出了限制条件来排除干扰项，优化了深度估计方式。

65、(5)本发明相对于稠密的直接法深度估计能够更好地满足实时性能要求。稠密的直接法主要还是利用基于关键帧在一个非凸优化的框架中最小化全局空间正则化能量函数来进行稠密的深度估计，其对于图像中每一个像素都有极大的计算量，因此很难达到实时的性能。而本发明在块匹配过程中首先过滤了一些图像块梯度不符合要求的块匹配的ncc计算，因此相较于稠密的直接法，本发明能够更好的达到实时性的要求。

66、(6)本发明相较于使用rgb-d相机进行深度估计能够很好地被应用于纹理分布不均的室外及大场景场合中，同时降低功耗和成本。在纹理分布不均的室外大场景中，rgb-d相机会受到量程、光照或者其他传感器发射的红外光线的干扰。其次，在没有适当调制的情况下，同时使用多个rgb-d相机可能会出现相互干扰的问题，这会导致难以准确地获取深度信息。另外，同时使用多个rgb-d相机通常伴随着较高的成本和功耗。而对于传统的双目被动视觉，其虽没有量程的限制，但其无法避免纹理分布不均和动态物体对深度估计的影响。因此，本发明能够利用双目主动视觉很好地应用在纹理分布不均的室外及大场景场合中，同时降低功耗和成本。