CN117218210A - 一种基于仿生眼的双目主动视觉半稠密深度估计方法 - Google Patents
一种基于仿生眼的双目主动视觉半稠密深度估计方法 Download PDFInfo
- Publication number
- CN117218210A CN117218210A CN202311362069.4A CN202311362069A CN117218210A CN 117218210 A CN117218210 A CN 117218210A CN 202311362069 A CN202311362069 A CN 202311362069A CN 117218210 A CN117218210 A CN 117218210A
- Authority
- CN
- China
- Prior art keywords
- camera
- key frame
- value
- current
- binocular
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 239000011664 nicotinic acid Substances 0.000 title claims abstract description 120
- 238000000034 method Methods 0.000 title claims abstract description 78
- 230000004438 eyesight Effects 0.000 title claims abstract description 36
- 230000008569 process Effects 0.000 claims abstract description 23
- 238000005457 optimization Methods 0.000 claims description 34
- 239000011159 matrix material Substances 0.000 claims description 25
- 238000009826 distribution Methods 0.000 claims description 24
- 230000004927 fusion Effects 0.000 claims description 14
- 239000013598 vector Substances 0.000 claims description 14
- 230000000007 visual effect Effects 0.000 claims description 14
- 230000003287 optical effect Effects 0.000 claims description 12
- 230000009466 transformation Effects 0.000 claims description 12
- 238000013507 mapping Methods 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 10
- 238000010606 normalization Methods 0.000 claims description 9
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 5
- 238000010586 diagram Methods 0.000 claims description 4
- 238000012935 Averaging Methods 0.000 claims description 3
- 238000013459 approach Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000005315 distribution function Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 10
- 238000001514 detection method Methods 0.000 description 5
- 240000004282 Grewia occidentalis Species 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 3
- 238000010276 construction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 239000000047 product Substances 0.000 description 3
- 230000001131 transforming effect Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 2
- 238000007499 fusion processing Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 206010047571 Visual impairment Diseases 0.000 description 1
- 230000003592 biomimetic effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Image Processing (AREA)
Abstract
一种基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，包括以下步骤：S100，利用双目仿生眼相机采集图像；S200，将S100采集到的图像输入到SLAM系统中，利用SLAM系统追踪关键帧并估计相机位姿和路标点坐标；S300，利用S200获取的关键帧位姿和其对应关键帧图像数据，通过极线搜索、块匹配结合三角测量，再利用深度滤波器实现半稠密的深度估计。本发明在块匹配过程中首先过滤了一些图像块梯度不符合要求的块匹配的NCC计算，因此相较于稠密的直接法，本发明能够更好的达到实时性的要求。
Description
技术领域
本发明属于计算机视觉技术领域，涉及仿生眼运动控制、相机位姿与路标点坐标估计优化和半稠密深度估计问题，具体涉及一种基于仿生眼的双目主动视觉半稠密深度估计方法。
背景技术
在机器人领域，移动机器人在未知环境中的自主导航能力一直是一个备受瞩目且至关重要的研究方向。SLAM技术为移动机器人在未知环境中定位自身位置并构建地图提供了关键解决方案。双目被动视觉和单目被动视觉SLAM在目前SLAM领域中有较多研究。然而，在有动态物体、纹理分布不均的室外及大场景场合中，传统被动视觉SLAM仍然存在诸多的限制，导致算法的精度和鲁棒性受到影响甚至导致算法的失效。其原因是相机无法主动选择拍摄的场景，从而无法避免纹理分布较低区域和动态物体对于SLAM算法的负面影响。
目前，主流的特征点法视觉SLAM仅对稀疏的特征点的三维位置进行估计，最终生成的地图仅包含这些稀疏的路标点。虽然这种稀疏地图足以满足机器人在地图中进行重定位的需求，但对于一些更高的需求，如导航、避障和场景三维重建等，其表现受到限制。因此，具备半稠密深度估计的能力对于提高地图的可用性、推动SLAM技术的实际应用至关重要。如今，视觉SLAM主要通过相机特性、深度学习或直接法进行半稠密或者稠密的深度估计，但目前在相关机器人上应用最多的还是使用RGB-D相机通过TOF(Time ofFlight)法或者结构光法等直接获取图像中所有像素的深度值，其原因是RGB-D相机使用较为容易且其可以直接通过传感器中硬件测量深度，无需消耗大量的计算资源来估计。然而，RGB-D相机的稠密建图只适用于一些特定的场景中，在室外或是大场景场合中，RGB-D相机还无法被很好地应用，且其功耗和成本较高。
发明内容
本发明的目的旨在提供一种基于仿生眼的双目主动视觉半稠密深度估计方法。
基于上述目的，本发明采取了如下技术方案：
一种基于仿生眼的双目主动视觉半稠密深度估计方法，包括以下步骤：
S100，利用双目仿生眼相机采集图像；
S200，将S100采集到的图像输入到SLAM系统中，利用SLAM系统追踪关键帧并估计相机位姿和路标点坐标；
S300，利用S200获取的关键帧位姿和其对应关键帧图像数据，通过极线搜索、块匹配结合三角测量，再利用深度滤波器实现半稠密的深度估计，具体是：
S301，选定进入步骤S300的一帧为参考关键帧，将参考关键帧所对应图像数据中所有像素深度所对应的逆深度假设为满足一个初始的高斯分布，为后续的深度信息融合提供一个初值；
S302，插入新的关键帧，通过极线搜索和块匹配确定投影点位置；
S303，根据参考关键帧和当前关键帧之间的几何关系计算三角化后的三维点深度及其不确定性；
S304，将当前观测深度所对应逆深度的高斯分布融合进上一次的估计中建立融合后逆深度的分布；若不确定性小于一定阈值，则认为深度数据收敛，确定三维点深度，否则返回步骤S302继续进行信息融合。
所述步骤S302具体是：
设参考关键帧所对应图像中的一个像素为u1，该像素所对应的深度范围在当前关键帧所对应图像中的投影为一条极线l2；将参考关键帧所对应图像记为Ik，当前关键帧所对应图像记为It，世界坐标系相对于参考关键帧和当前关键帧的旋转矩阵分别为RkW和RtW，世界坐标系相对于参考关键帧和当前关键帧的平移向量分别为tkW和ttW，通过标定得左相机的内参KL，则参考关键帧和当前关键帧所对应图像在像素坐标系下的相互关系为：
其中，uk为参考关键帧中的一个像素，dk为该像素所对应的空间点深度；当得知dk、uk时，就可以计算出ut的位置；当给uk的两个分量再增加一个增量du、dv时，可以算出ut的增量dut、dvt，由此得到局部范围内关键帧和当前帧图像坐标变换的一个线性关系构成的仿射变换：
接着，根据仿射变换矩阵，将相机的旋转考虑进块匹配：
取u1周围的像素块为同时把l2上的像素块记为/>使用去均值的归一化互相关方式比较u1周围像素块和极线上像素块的相关性；当像素块梯度与极线所夹的夹角较大时，块匹配的可信度较低，因此此处只匹配像素梯度在极线方向或在极线方向一定阈值内的像素块，根据NCC的定义，有：
确定步长后，通过遍历对应极线上规定像素块与源像素块的NCC值，可以得到一个非凸的NCC分布函数，寻找NCC最高的点作为源像素块的匹配像素块。
步骤S302中，若块匹配到的最高NCC值仍低于阈值，则认为此当前关键帧所对应图像中对于源像素块的极线搜索与块匹配失效，进入新的关键帧以及与之对应且经S200之SLAM系统优化后的相机位姿后，再进行匹配与信息融合收敛。
所述步骤S303中，通过三角测量对三维点深度进行计算，计算方式如下：
设有两帧图像分别为I1与I2，以I1作为参考，I2的变换矩阵为T；相机光心为O1与O2；在I1中有一个特征点p1，对应I2中有特征点p2，连接光心O1与特征点p1，光心O2与特征点p2，得到射线O1p1与O2p2；在相机不作纯平移运动时，直线O1p1与O2p2在场景中相交于一点P，该点是两个特征点p1与p2所对应的地图点在三维场景中的位置；
设x1与x2为特征点p1与p2所对应的归一化坐标，则其关系满足：
s2x2＝s1Rx1+t，其中s1与s2分别为两个特征点p1与p2的深度，旋转矩阵R和平移向量t已知；
从几何角度，可以在射线O1p1上寻找三维点使其投影位置接近p2，同理也可以在射线O2p2上寻找；
以计算s1为例，变换上式得到只关于s1的方程：解方程直接求得s1；同理求得s2；此时初步确定两个特征点的深度；
因噪声影响O1p1与O2p2无法相交时，通过最小二乘法求解。
步骤S303中，当不断出现新的当前关键帧利用极线搜索和块匹配来找到对应像素块并利用三角测量来估计深度时，都会计算其在参考关键帧中的观测深度并转化为逆深度，此时的逆深度同样为一个高斯分布：
其中，/>通过极线搜索后像素的误差结合三角测量几何关系计算出的深度偏差而获得。
所述步骤S100中，采用全景相机对双目仿生眼相机进行注视引导，步骤包括：
S101，获取全景图像，然后检测全景图像中影响SLAM任务执行的关键场景信息，包括特征点以及动态物体；
S102，基于关键场景信息对全景图像中的像素点进行赋值，构建全景价值图像；
S103，将左右两个仿生眼相机的视场区域分别投影到全景价值图像中，得到左视场投影区域和右视场投影区域；同时控制左视场投影区域和右视场投影区域重叠；合并左视场投影区域和右视场投影区域，得到当前双目视场投影区域；
S104，在全景价值图像中得到当前双目视场投影区域的价值均值；判断当前双目视场投影区域的价值均值与价值阈值的大小：若当前双目视场投影区域的价值均值大于价值阈值，则不移动当前双目视场投影区域，并将左右两个仿生眼相机当前采集的高价值图像作为SLAM系统的输入；若当前双目视场投影区域的价值均值小于等于价值阈值，则在全景价值图像中搜索比价值阈值更高的目标双目视场投影区域，然后进入步骤S105；
S105，根据当前双目视场投影区域和目标双目视场投影区域，分别计算左右两个仿生眼相机所需的位移量；根据位移量分别移动左右两个仿生眼相机，最后将左右两个仿生眼相机在所述目标双目视场投影区域中采集的高价值图像作为SLAM系统的输入。
所述步骤S104中，
通过在全景图像可注视区域中采用与当前仿生眼相机注视区域投影大小相同的矩形作为滑动窗口，从当前注视区域以一定步长s由近致远进行搜索，再结合全景图像价值，得到每个窗口中的价值均值Vm；
定义价值阈值Vth，若当前注视区域的价值均值高于价值阈值，选择不移动相机；反之，若当前注视区域的价值均值小于等于价值阈值，选择将视线转移至价值均值高于价值阈值Vth且与当前注视区域距离最近的区域。
所述步骤S105中，将左仿生眼相机的旋转运动表示为旋转矩阵RLCLT，右相机的旋转运动表示为旋转矩阵RRCRT，通过如下方法求解RLCLT：
将全景价值图中目标注视区域的中心像素坐标uPT根据投影模型反投影至全景相机投影球面中，并将反投影至球面的投影点PPT转换至左相机初始位姿坐标系OLO中获得左相机的目标光轴方向PLOT：/>将左仿生眼相机目前注视区域的像素平面中心点/>通过相机投影模型/>反投影至归一化平面，再将其转换为左仿生眼相机的初始位姿坐标系OLO中获得左相机在初始位姿坐标系OLO中的当前姿态PLOC：由此可得，左仿生眼相机需要从当前姿态PLOC移动到目标姿态PLOT；
计算当前姿态PLOC与目标姿态PLOT之间旋转轴：
根据点乘的定义，对于旋转角度θLCLT：
根据旋转角度和旋转轴，通过罗德里格斯公式可得旋转矩阵PLCLT：
同理求解RRCRT；
通过求解左右相机的旋转矩阵，获得仿生眼控制模块需要对左、右仿生眼控制电机下达的旋转指令，使仿生眼相机将视域转移至场景中的高价值区域，实现注视引导。
所述SLAM系统采用ORB-SLAM2框架改进的SLAM算法，包括四个模块，分别是追踪线程、局部建图线程、回环闭合线程和全局BA线程：
(1)通过将当前帧与局部地图进行特征点匹配，实现每一帧的跟踪，利用运动模式、PnP重定位模式或参考关键帧模式初始化相机位姿并追踪局部地图，采用PnP最小化重投影误差来估计相机的位姿，同时判断关键帧，作为追踪线程运行；
(2)管理局部地图并通过局部建图线程执行局部BA优化；
(3)使用DBoW2库检测大型回环，并通过执行位姿图优化来校正相机轨迹累积误差，以此构成回环闭环线程；
(4)在完成位姿图优化后，启动全局BA线程，以获取全局一致的相机位姿和地图结构。
所述SLAM系统中，若当前帧满足下列任意一个条件，将当前帧设置为关键帧：
(1)当前帧为进行仿生眼视线控制到达目标注视区域后的第一帧；
(2)距离上一次全局重定位超过13帧；
(3)局部建图线程处于空闲状态；
(4)距离上一个设置的关键帧超过15帧；
(5)当前帧与上一个设置的关键帧之间的平移距离超过阈值tth；
(6)当前帧中追踪成功的特征点的数量达到70以上；
(7)当前帧中追踪成功的特征点数量少于参考关键帧的85％。
与现有技术相比，本发明具有如下有益效果：
(1)本发明基于人眼的视觉机制，当场景中的特征点分布不均时，能通过构建全景价值图将特征点相对密集的区域赋予高价值，从而引导仿生眼向此区域移动。密集的特征点有利于仿生眼相机的位姿估计优化和全局地图的构建，提高了SLAM系统的稳定性。同时也为后续仿生眼相机利用块匹配进行半稠密深度估计提供了纹理基础。
(2)本发明在构建全景价值图的过程中排除了动态物体对于相机状态估计优化和半稠密深度估计的干扰，若在半稠密深度估计的过程中出现了动态物体，则会影响块匹配过程，动态物体移动后，对应极线上原应匹配的像素点消失，进而匹配错误的像素块从而导致匹配失效或深度估计错误。因此，排除动态物体这一步骤为后续半稠密深度估计提供了质量更好的图像数据。
(3)本发明对于仿生眼相机相对位姿变换时的位姿估计和优化提供了一种改进后的优化算法，能够很好地通过四个线程来优化相机的位姿和左右仿生眼相机的相对位姿及路标点坐标，为后续半稠密深度估计步骤提供了基础。
(4)本发明利用了上述的优点进行了半稠密深度估计，排除了对深度估计过程的多数干扰项并提供了良好的数据。同时，为了更准确地进行深度估计，本发明对像素块梯度做出了限制条件来排除干扰项，优化了深度估计方式。
(5)本发明相对于稠密的直接法深度估计能够更好地满足实时性能要求。稠密的直接法主要还是利用基于关键帧在一个非凸优化的框架中最小化全局空间正则化能量函数来进行稠密的深度估计，其对于图像中每一个像素都有极大的计算量，因此很难达到实时的性能。而本发明在块匹配过程中首先过滤了一些图像块梯度不符合要求的块匹配的NCC计算，因此相较于稠密的直接法，本发明能够更好的达到实时性的要求。
(6)本发明相较于使用RGB-D相机进行深度估计能够很好地被应用于纹理分布不均的室外及大场景场合中，同时降低功耗和成本。在纹理分布不均的室外大场景中，RGB-D相机会受到量程、光照或者其他传感器发射的红外光线的干扰。其次，在没有适当调制的情况下，同时使用多个RGB-D相机可能会出现相互干扰的问题，这会导致难以准确地获取深度信息。另外，同时使用多个RGB-D相机通常伴随着较高的成本和功耗。而对于传统的双目被动视觉，其虽没有量程的限制，但其无法避免纹理分布不均和动态物体对深度估计的影响。因此，本发明能够利用双目主动视觉很好地应用在纹理分布不均的室外及大场景场合中，同时降低功耗和成本。
附图说明
图1为实施例中基于仿生眼的双目主动视觉半稠密深度估计方法的整体流程图；
图2为实施例中利用全景相机对双目仿生眼相机进行视觉引导的流程图；
图3为实施例中利用SLAM系统追踪关键帧并估计相机位姿和路标点坐标的流程图；
图4为实施例中半稠密深度估计的流程图；
图5为三角测量示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。
如附图1所示，本实施例提供了一种更加具体化的、基于仿生眼的双目主动视觉半稠密深度估计方法，步骤包括：
S100，利用全景相机对双目仿生眼相机进行视觉引导，由双目仿生眼相机采集高价值图像，如图2所示，过程如下：
S101，采用一个全景相机捕捉周围环境信息，从全景相机采集到的全景图像中，进行影响SLAM运行精度和鲁棒性关键信息的检测，包括特征点以及动态物体：
在全景图像中，为了达到与后续双目主动SLAM算法的匹配，使后续双目主动SLAM也能提取到相同的特征点，保证特征点构成的全景价值图的有效性，同时达到算法的实时性，此处在全景图像中，采用FAST关键点检测算法来提取特征点。由于全景价值图只是提供对于特征点的信息进而控制双目仿生眼的运动，因此省略了SLAM算法提取的ORB特征点中的BRIRF描述子计算。
对于严重破坏SLAM算法性能的动态物体，应该尽量避免其出现在SLAM算法的输入图像中，因此首先要检测图像中的动态物体。利用现有动态物体检测算法可以检测出全景图像中的行人、动物等或对SLAM算法具有负面作用的潜在动态物体，能够尽量排除动态物体的干扰。
S102，基于关键场景信息对全景图中的各个区域进行评价，以确定各个区域对SLAM算法的价值，对全景图像中的像素点进行赋值，构建全景价值图：
根据全景图像中提取的特征点，对于每一个特征点及其周边领域的像素赋予较高的价值。价值随着领域半径的增大而逐渐降低。若在特征点比较集中的区域中，同一个像素点被多次赋值，那么该像素的价值取其被赋予的最大值。
进一步地，将上述所述动态物体检测目标框中的所有像素均赋予低价值，甚至可以使其价值低于没有检测出特征点的区域，以此来排除动态物体对于后续SLAM算法性能的影响。
S103，将左右两个仿生眼相机的视场区域分别投影到全景价值图像中，得到左视场投影区域和右视场投影区域；同时控制左视场投影区域和右视场投影区域重叠；合并左视场投影区域和右视场投影区域，得到当前双目视场投影区域：
在此，将全景相机相对于左相机的初始位姿坐标系记为TLOP，将全景相机相对于右相机的初始位姿坐标系记为TROP，并且通过标定可以确定这两个变换矩阵。根据仿生眼控制模块的反馈，左右两个仿生眼相对于其初始位姿TLOL和TROR都可实时获得。定义两个仿生眼相机的归一化平面投影方程为全景相机球面至像素平面的投影方程为/>此处以左相机为例，左仿生眼相机的四角像素点uL(0,0)、uL(m，0)、uL(m，n)和uL(0，n)(其中m，n分别为仿生眼相机像素平面宽度W和高度H方向的最大像素坐标)在其归一化平面PL上的投影：
由此可以得到左仿生眼相机四角像素点在归一化平面PL上的投影PL(0,0)、PL(m，0)、PL(m，n)和PL(0，n)，右仿生眼相机同理。
进一步地，将左相机归一化平面上的四角投影点变换至全景相机的坐标系中：
由于仿生眼相机只有两个自由度，分别为左右旋转运动和上下俯仰运动，因此其当前位姿坐标系与初始位姿坐标系的原点重合，且全景相机和仿生眼相机原点的相对位置始终保持不变，可从标定获得的TLOP中取得左仿生眼相机相对于全景相机的平移向量tPL0。将tPL0和PL(0，0)、PL(m，0)、PL(m，n)和PL(0，n)这四个点分别连接，构造四条直线方程LP(0，0)、LP(m，0)、LP(m，n)、LP(0，n)，这四条直线方程的表达形式为：
其中，xPL0为左仿生眼相机原点坐标变换至全景相机坐标系中的x方向坐标，yPL0为左仿生眼相机原点坐标变换至全景相机坐标系中的y方向坐标，zPL0为左仿生眼相机原点坐标变换至全景相机坐标系中的z方向坐标。
全景相机球面投影的归一化球面方程为：
x2+y2+z2＝1 (4)，
联立方程(3)和(4)，就可以求出四条直线与全景相机投影球面的交点，每条直线与球面会形成两个交点，但是由于仿生眼相机受限于运动范围无法看到全景相机坐标系中x＜0的场景，因此舍去x＜0的结果，保留x＞0的交点。因此，四条直线与全景相机投影球面的交点为PP1、PP2、PP3和PP4。
进一步地，通过全景相机投影球面到像素平面的投影方程将四个交点投影至全景相机的像素平面上：
将四个投影点连接，由于全景相机投影过程中的畸变问题，连接之后为一个不规则四边形，该四边形即为左仿生眼相机在全景图像中的投影。同理可得右仿生眼相机在全景图像中的投影。根据仿生眼相机的机械结构限制，可得仿生眼相机在全景图像中所能覆盖的运动范围。同时，为了使拍摄的图像都位于左右仿生眼相机的运动范围内，需进一步限制左右仿生眼相机的可运动范围。需要指出的是，为了方便全景价值算法的计算，本阶段将整合后的注视区域投影近似为其最大外接矩形；这样做的目的是简化计算过程，将复杂的不规则形状近似为一个矩形，从而在后续的算法中更方便地处理。
S104，在全景价值图像中得到当前双目视场投影区域的价值均值；判断当前双目视场投影区域的价值均值与价值阈值的大小：若当前双目视场投影区域的价值均值大于价值阈值，则不移动当前双目视场投影区域，并将左右两个仿生眼相机当前采集的高价值图像作为SLAM系统的输入；若当前双目视场投影区域的价值均值小于等于价值阈值，则在全景价值图像中搜索比价值阈值更高的目标双目视场投影区域，然后进入步骤S105。
通过在全景图像可注视区域中采用与当前仿生眼相机注视区域投影大小相同的矩形作为滑动窗口，从当前注视区域以一定步长s由近致远进行搜索，再结合上述所说的全景图像价值，可以得到每个窗口中的价值均值Vm。其中为了减小相机旋转中产生的抖动对于SLAM算法的影响，此处定义价值阈值Vth，若当前注视区域的价值均值高于价值阈值(即Vm＞Vth)，意味着当前注视区域已满足SLAM算法良好的运行需求，此时选择不移动相机。反之，若Vm≤Vth，则意味着当前场景中可提取的特征点数量不足或者是场景中包含了动态物体，为了提高SLAM算法的运行质量，选择将视线转移至价值均值高于价值阈值Vth且与当前注视区域距离最近的区域。
S105，根据当前双目视场投影区域和目标双目视场投影区域，分别计算左右两个仿生眼相机所需的位移量；根据位移量分别移动左右两个仿生眼相机，最后将左右两个仿生眼相机在所述目标双目视场投影区域中采集的高价值图像作为SLAM系统的输入。
如前所述，仿生眼具有两个自由度，分别为左右旋转运动和上下俯仰运动，相对于机器人平台只发生了纯转动，因此可以将左仿生眼相机的旋转运动表示为旋转矩阵RLCLT，右相机的旋转运动表示为旋转矩阵RRCRT。
以左相机为例，将全景价值图中目标注视区域的中心像素坐标uPT根据投影模型反投影至全景相机投影球面中，并将反投影至球面的投影点PPT转换至左相机初始位姿坐标系OLO中获得左相机的目标光轴方向PLOT：
进一步地，将左仿生眼相机目前注视区域的像素平面中心点通过相机投影模型/>反投影至归一化平面，再将其转换为左仿生眼相机的初始位姿坐标系OLO中获得左相机在初始位姿坐标系OLO中的当前姿态PLOC：
由此可得，左仿生眼相机需要从当前姿态PLOC移动到目标姿态PLOT。
进一步地，计算左仿生眼相机从当前姿态PLOC移动到目标姿态PLOT之间的旋转矩阵RLCLT。首先根据这两个表示姿态的向量计算二者之间的旋转向量。
确定两个向量之间的旋转轴：
由于旋转向量中旋转轴为单位向量，因此上式中对叉乘后的向量进行了归一化。根据点乘的定义，对于旋转角度θLCLT：
根据旋转角度和旋转轴，通过罗德里格斯公式可得旋转矩阵RLCLT：
通过求解左相机的旋转矩阵RLCLT，可以获得仿生眼控制模块需要对左仿生眼控制电机下达的旋转指令，使仿生眼相机将视域转移至场景中的高价值区域，达到主动视觉的目的。同理，也可以求得右相机的旋转矩阵进而控制右相机的旋转以此来提高SLAM算法的运行质量。
S200，将S100采集到的高价值图像输入到SLAM算法中，利用SLAM算法估计相机位姿和路标点坐标。
如图3所示，本实施例采用了基于ORB-SLAM2框架改进的SLAM算法，以使左右仿生眼相机发生相对运动时仍能保持稳定运行，并在相机的位姿和路标点坐标估计方面具有更高的准确性。
SLAM算法包括四个模块，分别是追踪线程、局部建图线程、回环闭合线程和全局BA线程，具体是：
S201，追踪线程，对相机位姿进行估计并判断关键帧：
首先构建初始的局部地图：左右仿生眼相机采集的图像输入SLAM算法框架后，在预处理模块中提取两幅图像的ORB特征点，后续所有算法模块的操作都基于提取的ORB特征点，图像的表达从像素点的集合转变为ORB特征点的集合，降低运行中缓存的数据量。根据标定获得的左右仿生眼之间的位姿TL0R0，进一步通过双目视差，构建初始的局部地图。
其次，设定左相机为主相机，将左相机输入的预处理图像经过ORB特征点提取，利用运动模式、PnP重定位模式或参考关键帧模式初始化左相机位姿，此时初始化估计的位姿精度较低，需进一步追踪局部地图对位姿估计进一步优化。在局部地图中检索与当前左相机帧的ORB特征点匹配，然后构建PnP问题利用最小化重投影误差求解，从而优化左相机位姿。
对于非线性优化中定义的误差项：
其中ui表示观测到的点P的像素坐标，Pi表示局部地图中点P的三维坐标；e表示重投影误差；以相机位姿为例，构建最小二乘问题时，需要求得最佳相机位姿：
此最小二乘问题使用李代数可以构建无约束的优化问题，很方便地通过优化算法进行求解；在迭代优化时，通过重投影误差关于相机位姿的导数，也就是线性化目标函数所需的雅可比矩阵：
可求解左相机位姿的更新量。
进一步地，追踪线程的位姿估计任务中还需估计左右相机的相对位姿。上述仿生眼运动控制过程中已经获得左右仿生眼相对于其初始位姿坐标系的位姿TLOL和TROR，从而可以计算出左右仿生眼之间的相对位姿初始值TLR：
在左相机完成位姿估计后，成功地将其与局部地图中的路标点建立了匹配；此时，基于左右相机之间的ORB特征点匹配，以及左相机中ORB特征点与路标点的匹配，构建一个PnP问题最小化左右相机之间的特征点重投影误差获得一个优化后的左右相机相对位姿TLR；从而左相机的当前位姿和左右相机的相对位姿都得到了初步优化；此时利用经过优化后的左右相机相对位姿变换矩阵TLR，以及图像中已经获得的良好匹配特征点，可以进行三角测量得到大量优质双目匹配路标点。这些优质路标点作为局部地图中路标点的补充，提升了追踪线程的精度和鲁棒性。
最后是选取关键帧来为后续的线程服务；此处选取的关键帧指的是符合要求的一帧中左相机图像中所有的特征点，关键帧所对应的图像数据不参与SLAM算法，但会进入半稠密深度估计模块。若当前帧满足下列任意一个条件，将当前帧设置为关键帧：
(1)当前帧为进行仿生眼视线控制到达目标注视区域后的第一帧；
(2)距离上一次全局重定位超过13帧；
(3)局部建图线程处于空闲状态；
(4)距离上一个设置的关键帧超过15帧；
(5)当前帧与上一个设置的关键帧之间的平移距离超过阈值tth；
(6)当前帧中追踪成功的特征点的数量达到70以上；
(7)当前帧中追踪成功的特征点数量少于参考关键帧的85％。
通过关键帧的筛选为后续线程提供前提。
S202，局部建图线程，管理局部地图并执行局部BA优化，目的是更新剔除局部地图中的关键帧和路标点，同时优化关键帧所在的相机位姿和路标点的坐标。
首先是关键帧的更新；在追踪线程中，一旦判断当前帧满足设置为关键帧的条件，则将这个新关键帧与之前的关键帧进行关联；这个关联过程基于新关键帧与之前关键帧之间对路标点的共视关系，并基于DBoW词袋库计算新关键帧的词袋表达。
同时，对路标点进行更新与维护；如果一个路标点要保留在局部地图中，那它必须在其被创建之后的前三个关键帧中满足两个条件：
(1)必须在超过25％的根据位姿预测对该路标点可见的帧中被成功追踪；
(2)如果该路标点创建之后新加入了一个以上关键帧，则该路标点必须要被不少于三个关键帧观测到。
在局部建图线程中，局部BA优化本质上是将路标点的坐标也作为参数加入优化；与上述追踪线程中PnP最小化重投影误差优化相机位姿同理，通过重投影误差关于路标点坐标的导数，根据链式法则，有：
可获得路标点坐标的更新量，当迭代至目标函数误差收敛，便可获得局部地图中最优的关键帧所在的相机位姿和路标点的坐标。
进一步，为防止局部BA中数据规模逐渐累计变大影响局部建图线程的实时性能，进行关键帧的剔除；当一个关键帧中90％路标点可在至少三个其他关键帧中被观测到时，将该关键帧剔除，但其图像数据及其位姿仍会进入半稠密深度估计模块。
S203，回环闭合线程：使用DBoW2库检测大型回环，并通过执行位姿图(PoseGraph)优化来校正相机轨迹累积误差，以此构成回环闭合线程。此线程的主要任务是消除上述追踪线程和局部建图线程仅考虑相邻时间关键帧所产生的累计误差。
首先，在回环检测阶段，系统会检测可能的回环情况，找到候选回环关键帧；接着，计算当前关键帧与这些候选回环关键帧之间的相对位姿；借助它们之间的共视关系，将回环进行闭合；通过使用上述计算得到的当前关键帧与回环关键帧的相对位姿，对回环进行校正，进行位姿图(PoseGraph)优化，位姿图优化不考虑路标点的优化只考虑轨迹的优化，即构建一个只有轨迹的图优化；即，假设有Ki、Kj两个关键帧，在回环校正之前的相机位姿分别为TWi和TWj，在回环检测中对相机位姿进行校正后，可获得两帧之间的相对位姿Tij，由此构建误差eij：
基于该误差构建非线性优化，并求解误差项分别关于TWi和TWj的导数用于线性化，即可实现位姿图的优化。
S204，全局BA线程。在完成位姿图优化后，启动全局BA线程。此线程的主要任务是在无新数据输入后，利用BA优化最终精确轨迹和地图，获取全局一致的相机位姿和地图结构。
在回环闭合线程完成位姿图的优化后，会进行全局优化，以获取整个过程中全局最优的解；优化结束后通过生成树将更新后的关键帧相机位姿与未更新的关键帧相机位姿进行合并，路标点的坐标则根据其参考关键帧相机位姿的更新进行校正；若在全局BA优化的过程中检测到新的回环，则将线程终止，等待位姿图优化结束则再次启动。
S300，将S200获取的优化后的关键帧位姿和其对应关键帧图像数据输入半稠密深度估计模块，半稠密深度估计模块通过极线搜索、块匹配结合三角测量，再利用深度滤波器实现半稠密的深度估计，如图4所示，进而实现半稠密地图的构建，步骤包括：
S301，选定进入半稠密深度估计模块中的一帧为参考关键帧，将参考关键帧所对应图像数据中所有像素深度所对应的逆深度假设为满足一个初始的高斯分布。目的是为后续的深度信息融合提供一个初值。
以其中一帧参考关键帧所对应的图像数据为例，设某个像素的逆深度值为d-1且服从高斯分布：
P(d-1)＝N(μ，σ2) (17)，
此处逆深度分布的初始均值和方差对于收敛速度具有一定影响。
S302，当优化后的新关键帧位姿及其对应图像数据进入到半稠密深度估计模块时，利用优化后关键帧位姿进行极线搜索，进一步进行块匹配确定投影点位置。
设参考关键帧所对应图像中的一个像素为u1，该像素所对应的深度范围在当前关键帧所对应图像中的投影为一条极线l2。将参考关键帧所对应图像记为Ik，当前关键帧所对应图像记为It。世界坐标系相对于参考关键帧和当前关键帧的旋转矩阵分别为RkW和RtW，世界坐标系相对于参考关键帧和当前关键帧的平移向量分别为tkW和ttW，可由上述SLAM算法获得。同时通过上述标定可得左相机的内参KL，则参考关键帧和当前关键帧所对应图像在像素坐标系下的相互关系为：
其中，uk为参考关键帧中的一个像素，dk为该像素所对应的空间点深度。当得知dk、uk时，就可以计算出ut的位置；当给uk的两个分量再增加一个增量du、dv时，可以算出ut的增量dut、dvt，由此可以得到局部范围内关键帧和当前帧图像坐标变换的一个线性关系构成的仿射变换：
接着，根据仿射变换矩阵，将相机的旋转考虑进块匹配。在特征匹配方法中，需要根据描述子计算特征之间的匹配，然而此时无法把每个像素都当作特征点计算描述子。因此，需要通过比较亮度值的方式。由于单个像素的亮度几乎没有区分性，因此需要比较像素块的亮度，即采用块匹配的方式。
取u1周围的像素块为同时把l2上的像素块记为/>使用去均值的归一化互相关(Normalized Cross Correlation)方式比较u1周围像素块和极线上像素块的相关性，去均值的归一化互相关方式，允许像素块整体的亮度发生一些变化，相比较一般的归一化互相关方式更加可靠。
当像素块梯度与极线所夹的夹角较大时，块匹配的可信度较低，因此此处只匹配像素梯度在极线方向或在极线方向一定阈值内的像素块，根据NCC的定义，有：
确定步长后，通过遍历对应极线上规定像素块与源像素块的NCC值，可以得到一个非凸的NCC分布函数。寻找NCC最高的点作为源像素块的匹配像素块；若最高的匹配值也低于阈值，则认为此当前关键帧所对应图像中对于源像素块的极线搜索与块匹配失效，需要得到新图像与位姿数据后再进行匹配与信息融合收敛。
此处对像素块梯度的限制能够有效减少对于匹配过程中去均值归一化互相关的计算和对于像素块的误匹配，进而获得更为精确的三角化后深度。若不加限制，则会导致运算量的增加，同时块匹配的不确定性增大，得不到有效的匹配，从而可能导致错误的深度，进而影响后续的信息融合过程。
进一步地，不同当前关键帧与参考关键帧所对应图像通过块匹配和三角测量后得到的在参考关键帧中的估计深度可能会不相同，然而现实中三维空间中的一点所对应的坐标是确定的。因此需要引入深度滤波器，通过概率分布来描述深度值。
S303，根据参考关键帧和当前关键帧之间的几何关系计算三角化后的三维点深度及其不确定性，主要任务是提供新的观测深度所对应逆深度的高斯分布，以便后续的信息融合来使深度数据收敛。
对三维点深度的计算使用的是三角化(三角测量)方式，其示意图如图5所示，计算方式如下：
设有两帧图像分别为I1与I2，以I1作为参考，I2的变换矩阵为T。相机光心为O1与O2。在I1中有一个特征点p1，对应I2中有特征点p2，连接光心O1与特征点p1，光心O2与特征点p2，得到射线O1p1与O2p2。理论上，在相机不作纯平移运动时，直线O1p1与O2p2在场景中会相交于一点P，该点是两个特征点p1与p2所对应的地图点在三维场景中的位置。由于噪声的影响，这两条线可能无法相交，可通过最小二乘法求解。
设x1与x2为特征点p1与p2所对应的归一化坐标，则其关系满足：
s2x2＝s1Rx1+t (21)，
其中s1与s2分别为两个特征点p1与p2的深度，旋转矩阵R和平移向量t已知。从几何角度，可以在射线O1p1上寻找三维点使其投影位置接近p2。同理，也可以在射线O2p2上寻找。以计算s1为例，变换上式得到只关于s1的方程：
解上述方程可以直接求得s1。同理，s2也可以求出。此时可以初步确定两个特征点的深度。由于噪声的存在，需求解最小二乘解。
当不断出现新的当前关键帧利用极线搜索和块匹配来找到对应像素块并利用三角测量来估计深度时，都会计算其在参考关键帧中的观测深度并转化为逆深度。此时的逆深度同样为一个高斯分布：
其中，可以通过极线搜索后像素的误差结合三角测量几何关系计算出的深度偏差而获得。
S304，将当前观测深度所对应逆深度的高斯分布融合进上一次的估计中建立融合后逆深度的分布；若不确定性小于一定阈值时，认为深度数据收敛，否则需继续进行信息融合。步骤S303结合S304便可构成深度滤波器。
首先，通过融合新旧逆深度的高斯分布，根据高斯分布的乘积有：
通过迭代上述逆深度的观测计算和融合过程，当最新一次的信息融合得到的不确定性小于一定的阈值时，便可认为深度数据已经收敛。
进一步通过画图、读数据等最终可以得到一个半稠密的深度地图。
本发明依据人眼的视觉机制，在场景中特征点分布不均的情况下，通过构建全景价值图，将特征点密集的区域赋予较高的价值。这一策略能够引导仿生眼相机朝向这些区域移动。密集的特征点有助于位姿估计优化和全局地图构建，从而增强SLAM系统的稳定性。同时，特征点丰富区域也为后续使用块匹配进行半稠密深度估计提供了纹理基础，在有明显纹理的区域不易引起误匹配。其次，在构建全景价值图时剔除了动态物体对半稠密深度估计的干扰。若在半稠密深度估计的过程中出现了动态物体，则会影响块匹配过程，动态物体移动后，对应极线上原应匹配的像素点消失，进而匹配错误的像素块从而导致匹配失效或深度估计错误。因此，排除动态物体这一步骤为后续半稠密深度估计提供了质量更好的图像数据。
同时，针对双目主动视觉改进了仿生眼相机相对位姿变换时的SLAM算法。为后续半稠密深度估计提供了基础。
在半稠密深度估计模块，相较于传统的被动视觉双目半稠密深度估计，由于传统被动视觉双目相机之间基线是固定的，因此可以通过左右相机的视差来计算像素的深度。而在本发明中，双目仿生眼相机之间的相对位姿会发生变化，因此传统的利用视差的方式在此处失效。但本发明可以通过改进后的SLAM算法获得所筛选的关键帧位姿，因此可以通过关键帧位姿和其对应图像数据，利用极线搜索和块匹配来找到对应像素块并利用三角测量来估计深度，其中对于块匹配过程中像素块梯度的方向做出了一定限制，能够获得更为精确的三角化深度。同时，双目主动视觉相较于传统单目被动半稠密深度估计能够提供更多的纹理信息，这也能提高深度估计的精度。本发明在实际场景中具有重要的应用价值，为机器人在未知环境中的自主导航、定位能力提供了一种有效的方式。
Claims (10)
1.一种基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，包括以下步骤：
S100，利用双目仿生眼相机采集图像；
S200，将S100采集到的图像输入到SLAM系统中，利用SLAM系统追踪关键帧并估计相机位姿和路标点坐标；
S300，利用S200获取的关键帧位姿和其对应关键帧图像数据，通过极线搜索、块匹配结合三角测量，再利用深度滤波器实现半稠密的深度估计，具体是：
S301，选定进入步骤S300的一帧为参考关键帧，将参考关键帧所对应图像数据中所有像素深度所对应的逆深度假设为满足一个初始的高斯分布，为后续的深度信息融合提供一个初值；
S302，插入新的关键帧，通过极线搜索和块匹配确定投影点位置；
S303，根据参考关键帧和当前关键帧之间的几何关系计算三角化后的三维点深度及其不确定性；
S304，将当前观测深度所对应逆深度的高斯分布融合进上一次的估计中建立融合后逆深度的分布；若不确定性小于一定阈值，则认为深度数据收敛，确定三维点深度，否则返回步骤S302继续进行信息融合。
2.如权利要求1所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述步骤S302具体是：
设参考关键帧所对应图像中的一个像素为u1，该像素所对应的深度范围在当前关键帧所对应图像中的投影为一条极线l2；将参考关键帧所对应图像记为Ik，当前关键帧所对应图像记为It，世界坐标系相对于参考关键帧和当前关键帧的旋转矩阵分别为Rkw和Rtw，世界坐标系相对于参考关键帧和当前关键帧的平移向量分别为tkW和ttW，通过标定得左相机的内参KL，则参考关键帧和当前关键帧所对应图像在像素坐标系下的相互关系为：
其中，uk为参考关键帧中的一个像素，dk为该像素所对应的空间点深度；当得知dk、uk时，就可以计算出ut的位置；当给uk的两个分量再增加一个增量du、dv时，可以算出ut的增量dut、dvt，由此得到局部范围内关键帧和当前帧图像坐标变换的一个线性关系构成的仿射变换：
接着，根据仿射变换矩阵，将相机的旋转考虑进块匹配：
取u1周围的像素块为同时把l2上的像素块记为/>使用去均值的归一化互相关方式比较u1周围像素块和极线上像素块的相关性；当像素块梯度与极线所夹的夹角较大时，块匹配的可信度较低，因此此处只匹配像素梯度在极线方向或在极线方向一定阈值内的像素块，根据NCC的定义，有：
确定步长后，通过遍历对应极线上规定像素块与源像素块的NCC值，可以得到一个非凸的NCC分布函数，寻找NCC最高的点作为源像素块的匹配像素块。
3.如权利要求2所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，步骤S302中，若块匹配得到的最高NCC值仍低于阈值，则认为此当前关键帧所对应图像中对于源像素块的极线搜索与块匹配失效，进入新的关键帧以及与之对应且经S200之SLAM系统优化后的相机位姿后，再进行匹配与信息融合收敛。
4.如权利要求1所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述步骤S303中，通过三角测量对三维点深度进行计算，计算方式如下：
设有两帧图像分别为I1与I2，以I1作为参考，I2的变换矩阵为T；相机光心为O1与O2；在I1中有一个特征点p1，对应I2中有特征点p2，连接光心O1与特征点p1，光心O2与特征点p2，得到射线O1p1与O2p2；在相机不作纯平移运动时，直线O1p1与O2p2在场景中相交于一点P，该点是两个特征点p1与p2所对应的地图点在三维场景中的位置；
设x1与x2为特征点p1与p2所对应的归一化坐标，则其关系满足：
s2x2＝s1Rx1+t，其中s1与s2分别为两个特征点p1与p2的深度，旋转矩阵R和平移向量t已知；
从几何角度，可以在射线O1p1上寻找三维点使其投影位置接近p2，同理也可以在射线O2p2上寻找；
以计算s1为例，变换上式得到只关于s1的方程：解方程直接求得s1；同理求得s2；此时初步确定两个特征点的深度；
因噪声影响O1p1与O2p2无法相交时，通过最小二乘法求解。
5.如权利要求4所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，当不断出现新的当前关键帧利用极线搜索和块匹配来找到对应像素块并利用三角测量来估计深度时，都会计算其在参考关键帧中的观测深度并转化为逆深度，此时的逆深度同样为一个高斯分布：
其中，/>通过极线搜索后像素的误差结合三角测量几何关系计算出的深度偏差而获得。
6.如权利要求1所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述步骤S100中，采用全景相机对双目仿生眼相机进行注视引导，步骤包括：
S101，获取全景图像，然后检测全景图像中影响SLAM任务执行的关键场景信息，包括特征点以及动态物体；
S102，基于关键场景信息对全景图像中的像素点进行赋值，构建全景价值图像；
S103，将左右两个仿生眼相机的视场区域分别投影到全景价值图像中，得到左视场投影区域和右视场投影区域；同时控制左视场投影区域和右视场投影区域重叠；合并左视场投影区域和右视场投影区域，得到当前双目视场投影区域；
S104，在全景价值图像中得到当前双目视场投影区域的价值均值；判断当前双目视场投影区域的价值均值与价值阈值的大小：若当前双目视场投影区域的价值均值大于价值阈值，则不移动当前双目视场投影区域，并将左右两个仿生眼相机当前采集的高价值图像作为SLAM系统的输入；若当前双目视场投影区域的价值均值小于等于价值阈值，则在全景价值图像中搜索比价值阈值更高的目标双目视场投影区域，然后进入步骤S105；
S105，根据当前双目视场投影区域和目标双目视场投影区域，分别计算左右两个仿生眼相机所需的位移量；根据位移量分别移动左右两个仿生眼相机，最后将左右两个仿生眼相机在所述目标双目视场投影区域中采集的高价值图像作为SLAM系统的输入。
7.如权利要求6所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述步骤S104中，
通过在全景图像可注视区域中采用与当前仿生眼相机注视区域投影大小相同的矩形作为滑动窗口，从当前注视区域以一定步长S由近致远进行搜索，再结合全景图像价值，得到每个窗口中的价值均值Vm；
定义价值阈值Vth，若当前注视区域的价值均值高于价值阈值，选择不移动相机；反之，若当前注视区域的价值均值小于等于价值阈值，选择将视线转移至价值均值高于价值阈值Vth且与当前注视区域距离最近的区域。
8.如权利要求6所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述步骤S105中，将左仿生眼相机的旋转运动表示为旋转矩阵RLCLT，右相机的旋转运动表示为旋转矩阵RRCRT，通过如下方法求解RLCLT：
将全景价值图中目标注视区域的中心像素坐标uPT根据投影模型反投影至全景相机投影球面中，并将反投影至球面的投影点PPT转换至左相机初始位姿坐标系OLO中获得左相机的目标光轴方向PLOT：/>将左仿生眼相机目前注视区域的像素平面中心点/>通过相机投影模型/>反投影至归一化平面，再将其转换为左仿生眼相机的初始位姿坐标系OLO中获得左相机在初始位姿坐标系OLO中的当前姿态PLOC：由此可得，左仿生眼相机需要从当前姿态PLOC移动到目标姿态PLOT；
计算当前姿态PLOC与目标姿态PLOT之间旋转轴：
根据点乘的定义，对于旋转角度θLCLT：
根据旋转角度和旋转轴，通过罗德里格斯公式可得旋转矩阵RLCLT：
同理求解RRCRT；
通过求解左右相机的旋转矩阵，获得仿生眼控制模块需要对左、右仿生眼控制电机下达的旋转指令，使仿生眼相机将视域转移至场景中的高价值区域，实现注视引导。
9.如权利要求1-8任一所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述SLAM系统采用ORB-SLAM2框架改进的SLAM算法，包括四个模块，分别是追踪线程、局部建图线程、回环闭合线程和全局BA线程：
(1)通过将当前帧与局部地图进行特征点匹配，实现每一帧的跟踪，利用运动模式、PnP重定位模式或参考关键帧模式初始化相机位姿并追踪局部地图，采用PnP最小化重投影误差来估计相机的位姿，同时判断关键帧，作为追踪线程运行；
(2)管理局部地图并通过局部建图线程执行局部BA优化；
(3)使用DBoW2库检测大型回环，并通过执行位姿图优化来校正相机轨迹累积误差，以此构成回环闭环线程；
(4)在完成位姿图优化后，启动全局BA线程，以获取全局一致的相机位姿和地图结构。
10.如权利要求9所示的基于仿生眼的双目主动视觉半稠密深度估计方法，其特征在于，所述SLAM系统终，若当前帧满足下列任意一个条件，将当前帧设置为关键帧：
(1)当前帧为进行仿生眼视线控制到达目标注视区域后的第一帧；
(2)距离上一次全局重定位超过13帧；
(3)局部建图线程处于空闲状态；
(4)距离上一个设置的关键帧超过15帧；
(5)当前帧与上一个设置的关键帧之间的平移距离超过阈值tth；
(6)当前帧中追踪成功的特征点的数量达到70以上；
(7)当前帧中追踪成功的特征点数量少于参考关键帧的85％。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN2023111020205 | 2023-08-29 | ||
| CN202311102020 | 2023-08-29 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117218210A true CN117218210A (zh) | 2023-12-12 |
Family
ID=89044601
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311362069.4A Pending CN117218210A (zh) | 2023-08-29 | 2023-10-20 | 一种基于仿生眼的双目主动视觉半稠密深度估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117218210A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117456124A (zh) * | 2023-12-26 | 2024-01-26 | 浙江大学 | 一种基于背靠背双目鱼眼相机的稠密slam的方法 |
| CN117649454A (zh) * | 2024-01-29 | 2024-03-05 | 北京友友天宇系统技术有限公司 | 双目相机外参自动校正方法、装置、电子设备及存储介质 |
-
2023
- 2023-10-20 CN CN202311362069.4A patent/CN117218210A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117456124A (zh) * | 2023-12-26 | 2024-01-26 | 浙江大学 | 一种基于背靠背双目鱼眼相机的稠密slam的方法 |
| CN117456124B (zh) * | 2023-12-26 | 2024-03-26 | 浙江大学 | 一种基于背靠背双目鱼眼相机的稠密slam的方法 |
| CN117649454A (zh) * | 2024-01-29 | 2024-03-05 | 北京友友天宇系统技术有限公司 | 双目相机外参自动校正方法、装置、电子设备及存储介质 |
| CN117649454B (zh) * | 2024-01-29 | 2024-05-31 | 北京友友天宇系统技术有限公司 | 双目相机外参自动校正方法、装置、电子设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112258618B (zh) | 基于先验激光点云与深度图融合的语义建图与定位方法 | |
| Yang et al. | Monocular object and plane slam in structured environments | |
| CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
| Park et al. | Elastic lidar fusion: Dense map-centric continuous-time slam | |
| CN109211241B (zh) | 基于视觉slam的无人机自主定位方法 | |
| Kerl et al. | Robust odometry estimation for RGB-D cameras | |
| CN111210463B (zh) | 基于特征点辅助匹配的虚拟宽视角视觉里程计方法及系统 | |
| CN117218210A (zh) | 一种基于仿生眼的双目主动视觉半稠密深度估计方法 | |
| Alizadeh | Object distance measurement using a single camera for robotic applications | |
| CN107563323A (zh) | 一种视频人脸特征点定位方法 | |
| Theodorou et al. | Visual SLAM algorithms and their application for AR, mapping, localization and wayfinding | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| Grest et al. | Single view motion tracking by depth and silhouette information | |
| Sandy et al. | Object-based visual-inertial tracking for additive fabrication | |
| Agrawal et al. | PCE-SLAM: A real-time simultaneous localization and mapping using LiDAR data | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| Li et al. | Metric sensing and control of a quadrotor using a homography-based visual inertial fusion method | |
| Lin et al. | Contour-SLAM: A robust object-level SLAM based on contour alignment | |
| Fan et al. | A nonlinear optimization-based monocular dense mapping system of visual-inertial odometry | |
| CN117152228A (zh) | 基于通道自注意力机制的自监督图像深度估计方法 | |
| Li et al. | BA-LIOM: tightly coupled laser-inertial odometry and mapping with bundle adjustment | |
| Meng et al. | A tightly coupled monocular visual lidar odometry with loop closure | |
| Sizintsev et al. | Long-range augmented reality with dynamic occlusion rendering | |
| Kawasaki et al. | Motion estimation for non-overlapping cameras by improvement of feature points matching based on urban 3D structure | |
| CN114972491A (zh) | 视觉slam方法、电子设备、存储介质及产品 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |