CN111415375B - 一种基于多鱼眼摄像机和双针孔投影模型的slam方法 - Google Patents

一种基于多鱼眼摄像机和双针孔投影模型的slam方法 Download PDF

Info

Publication number
CN111415375B
CN111415375B CN202010132872.9A CN202010132872A CN111415375B CN 111415375 B CN111415375 B CN 111415375B CN 202010132872 A CN202010132872 A CN 202010132872A CN 111415375 B CN111415375 B CN 111415375B
Authority
CN
China
Prior art keywords
coordinate system
camera
pinhole
point
model
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.)
Active
Application number
CN202010132872.9A
Other languages
English (en)
Other versions
CN111415375A (zh
Inventor
李涛
哀薇
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202010132872.9A priority Critical patent/CN111415375B/zh
Publication of CN111415375A publication Critical patent/CN111415375A/zh
Application granted granted Critical
Publication of CN111415375B publication Critical patent/CN111415375B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/292—Multi-camera tracking
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30264—Parking
    • 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

  • Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本发明公开了一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，包括以下步骤：（1）标定摄像机内部和外部参数；（2）输入若干个鱼眼图像，如果系统尚未进行初始化，通过双针孔投影模型将输入图像校正为合适的双针孔透视图像，并且在每个视图的校正图像上提取特征；（3）结合摄像机外部参数将地图点转换到每个视图进行特征跟踪和匹配；（4）固定局部地图的地图点进行多视图姿态优化，更新估计的主体姿态；（5）联合局部地图的所有位姿和地图点进行多视图捆绑调整(bundle adjustment,BA)优化；（6）采用双向对称的查询方式，保证正、反向经过同一场景时都能正确检测到回环，回环检测成功后进行回环校正。

Description

一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法
技术领域
本发明属于计算机视觉技术领域，涉及一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法。
背景技术
随着无人驾驶的发展，针对特定场景的自主代客泊车系统(autonomous valetparking systems,AVPS)被认为最有希望早日实现商业落地，因此，被国内外各大车企和无人驾驶科技公司当成重点发展对象。其中，同时定位与地图构建(simultaneouslocalization and mapping,SLAM)技术作为实现自主定位的关键技术被应用于AVPS，特别是视觉SLAM(visual SLAM,vSLAM)越来越受欢迎，因为相比差分GPS、激光雷达等其他替代品，相机具有成本低、无源传感、丰富的环境信息等优点。
视觉SLAM已经被研究了几十年，目前主要有单目SLAM、双目SLAM和RGBD SLAM。单目SLAM硬件配置简单，但视野有限，真实尺度无法估计，如ORB-SLAM[Mur-Artal R,MontielJ M M,Tardos J D.ORB-SLAM:a Versatile and Accurate Monocular SLAM System[J].IEEE Transactions on Robotics,2015,31(5):1147-1163.]。双目SLAM可利用相机间的约束确定尺度信息，但需要更多的计算成本，如stereo-dso[Wang R,
Figure GDA0002527316840000011
Martin,Cremers D.Stereo DSO:Large-Scale Direct Sparse Visual Odometry with StereoCameras[J].2017.]。RGBD SLAM可同时获得彩色图像和深度图像，但测量范围小限制了近距离感知，如申请号为CN201811001614.6的中国专利《一种基于Kinect的视觉SLAM方法》。上述系统和专利由于针孔摄像机的视场受限，均存在诸如遮挡、跟踪易丢失等问题。Multicol-SLAM[Urban S,Hinz S.MultiCol-SLAM:A Modular Real-Time Multi-CameraSLAM System[J].2016.]采用多个鱼眼摄像机，直接使用原始鱼眼图像，并专门设计用于高度失真图像的新特征，但新特征过于耗时，且特征误匹配较多。D.Scaramuzza,A.Martinelli and R.Siegwart,“A Toolbox for Easy Calibrating OmnidirectionalCameras,”Proceedings of the IEEE International Conference on IntelligentRobots and Systems,Beijing,9-15October 2006.中的全向摄像机模型较为常用。Cubemap-SLAM[Wang Yahui,Cai Shaojun,Li Shijie,et al.,2019.CubemapSLAM:APiecewise-Pinhole Monocular Fisheye SLAM System.10.1007/978-3-030-20876-9_3.]采用单个大视场鱼眼摄像机，并使用立方体图模型消除鱼眼图像畸变，但仍然存在单目尺度问题。
发明内容
针对上述存在的问题，本发明提出了一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，利用多个鱼眼摄像机扩大视场，构建双针孔投影模型消除图像畸变，实现高精度多目初始化、前端跟踪和后端优化等算法，保证全方位观察无人车周围的环境，消除尺度问题，改善实时性和特征匹配。
本发明至少通过如下技术方案之一实现。
一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，包括以下步骤：
(1)标定摄像机内部和外部参数，其中摄像机内部参数是指标定所使用的鱼眼相机模型中涉及的所有参数，外部参数是指从摄像机坐标系到多相机系统模型中主体坐标系的刚性变换所涉及到的参数；
(2)输入若干个鱼眼图像，如果系统尚未进行初始化，首先利用双针孔投影模型和双目测距原理完成多目SLAM初始化；
(3)如果已经完成初始化，通过双针孔投影模型将输入图像校正为合适的双针孔透视图像，并且在每个视图的校正图像上提取特征；
(4)利用运动模型或重定位估计初始的主体位姿，结合摄像机外部参数将地图点转换到每个视图进行特征跟踪和匹配；
(5)固定局部地图的地图点进行多视图姿态优化，更新估计的主体姿态；
(6)完成多关键帧插入、新地图点生成和局部地图更新后，联合局部地图的所有位姿和地图点进行多视图捆绑调整(bundle adjustment,BA)优化；
(7)针对多视图的特点，采用双向对称的查询方式，保证正、反向经过同一场景时都能正确检测到回环，回环检测成功后进行回环校正。
进一步地，所述步骤(1)中，在多相机系统模型引入主体坐标系，主体坐标系是表示多相机系统绝对位姿的中间坐标系，既将每个摄像机的观测值单独分开，同时将所有观测值组合为一个观测方程式；多相机系统模型包含三种坐标系：世界坐标系(world)、主体坐标系(body)和相机坐标系(camera)，并且将坐标系标记在符号的右下角，其数学表达式如下：
Figure GDA0002527316840000031
其中，刚性变换T在三维空间R3中被参数化为轴角旋转矢量r和平移矢量t，它将3D点X转换为T*X＝R(r)X+t，R(r)为r的3×3旋转矩阵，*表示变换的组成，
Figure GDA0002527316840000032
表示在时刻t从主体坐标系b到第i个相机坐标系ci的刚性变换，
Figure GDA0002527316840000033
表示在时刻t从世界坐标系w到主体坐标系b的刚性变换，
Figure GDA0002527316840000034
分别表示在时刻t相机坐标系c和世界坐标系w下的一个点。
进一步地，步骤(2)中，所述若干个鱼眼图像是经过时间同步的同一时刻的若干个图像，并且来自相邻摄像机的图像之间存在重叠区域。
进一步地，步骤(2)中，所述双针孔投影模型包含两部分，每个部分都是独立的针孔模型，各自的成像平面相交形成一定的夹角，交线是投影平面的分界线，从而确保视图之间的重叠区域是透视的，并且在工程上易于实现，双针孔投影模型的数学表达式如下：
x＝KRpcXc,
其中，Xc为相机坐标系下的3D点，x为投影平面上的像素坐标，K为针孔相机的内参矩阵，Rpc为从相机坐标系到投影平面坐标系(plane)的3×3旋转矩阵。
进一步地，所述步骤(2)的初始化是利用摄相机外部参数结合双针孔投影模型，对视图间的重叠区域进行立体校正，通过立体匹配策略来执行视图间的特征匹配，然后对所有的匹配特征点对进行三角测量，生成初始地图。
进一步地，所述步骤(4)中，所述特征跟踪和匹配的过程如下：
获取前一个多关键帧中对应的所有地图点，利用初始的主体位姿将这些地图点投影到当前多关键帧中每个视图的像平面，再根据设置的搜索半径在投影像素点附近确定搜索区域，取出搜索区域内的所有特征点作为候选匹配点集，计算地图点的描述子和候选匹配点集的描述子之间的距离，获得描述子距离最近且介于上阈值和下阈值之间的候选特征点作为最佳匹配。
进一步地，所述步骤(5)中，所述多视图姿态优化使用的代价函数为：
Figure GDA0002527316840000041
其中，
Figure GDA0002527316840000042
为从主体坐标系b到相机j坐标系cj的变换，
Figure GDA0002527316840000043
为相机j中的第i项，
Figure GDA0002527316840000044
为相机j中的第i个像素点，
Figure GDA0002527316840000045
为相机j中的第i个匹配地图点，π0(·)为双针孔投影模型，ρ为异常值影响最小化的柯西(Cauchy)损失函数，Tbw表示从世界坐标系w到主体坐标系b的刚性变换，|| ||2表示二范数的平方。
进一步地，所述步骤(6)中，所述多视图BA优化使用的代价函数为：
Figure GDA0002527316840000046
其中，
Figure GDA0002527316840000047
表示t时刻相机j中的第i个像素点，
Figure GDA0002527316840000048
表示t时刻相机j中的第i个匹配地图点。
进一步地，所述步骤(5)(6)中，对于优化任务，采用在g2o求解器中添加新的优化边，并提供解析形式的雅可比，通过整合多相机系统模型和双针孔投影模型，得到从世界点Xw到像素点x的最终方程为：
x＝KRpcTcb*Tbw*Xw,
其中，x为投影平面上的像素点，Xw为世界坐标系w下的3D点，K为针孔相机的内参矩阵，Rpc为从相机坐标系c到投影平面坐标系p的3×3旋转矩阵，Tcb为从主体坐标系b到相机坐标系c的刚性变换，Tbw为从世界坐标系w到主体坐标系b的刚性变换。
使用李代数
Figure GDA0002527316840000051
表示刚性变换T∈SE(3)，其中
Figure GDA0002527316840000052
为特殊欧式群SE(3)对应的李代数，φ为特殊正交群SO(3)对应的李代数，ρ为三维平移向量，上标T表示转置，并令观测误差e＝x-KRpcTcb*Tbw*Xw，则解析形式的雅可比为：
Figure GDA0002527316840000053
Figure GDA0002527316840000054
其中，x＝KXp，Xp＝RpcXc，Xc＝TcwXw，Tcw＝Tcb*Tbw，Tcw为从世界坐标系w到相机坐标系c的刚性变换，Tbw为从世界坐标系w到主体坐标系b的刚性变换，ε为Tcw对应的李代数，ξ为Tbw对应的李代数，Jξ为观测误差e对ξ的导数，
Figure GDA0002527316840000055
为观测误差e对Xw的导数，Xw为世界坐标系w下的3D点，Xp为投影平面坐标系p下的3D点，Xc为相机坐标系c下的3D点，
Figure GDA0002527316840000056
为Xc的斜对称矩阵，I3×3为3×3的单位阵，
Figure GDA0002527316840000057
为刚性变换Tcb∈SE(3)的伴随表示，
Figure GDA0002527316840000058
R为刚性变换T的旋转部分，τ为刚性变换T的平移部分，Rcb为刚性变换Tcb的旋转部分，Rbw为刚性变换Tbw的旋转部分。
进一步地，所述步骤(7)中，所述回环检测是借助大视场和摄像机对称分布的天然优势，当反向经过正向观测的同一地方时，由于观测角度刚好相反，原观测场景的摄像机无法再次观测，换成被与其对称分布的摄像机观测到，使得在当前关键帧和待回环候选帧上两者之间为相同或对称关系，因此查询回环候选帧时，仅需搜索相同或对称位置的关键帧。
与现有的技术相比，本发明具的有益效果如下：
(1)采用多个鱼眼摄像机，能够全方位观察无人车周围的环境，最大限度地提高姿态精度，同时最小化无人车被遮挡的图像区域；
(2)采用双针孔投影模型，能够在消除图像畸变的同时，保证图像信息不丢失，连续跟踪每个视图中的特征点，并跨视图保持特征描述符的一致性；
(3)采用多相机系统模型，能够将每个摄像机的观测值单独分开，也能够方便地将多个摄像机的观测值组合为一个观测方程式；
(4)采用双向对称的回环查询策略，充分利用大视场、多视图的特点，保证正、反向经过同一场景时都能正确检测到回环，提高系统的鲁棒性和准确性；
(5)充分利用相邻摄像机之间形成的重叠区域，基于双针孔投影模型和双目测距原理，解决多目SLAM系统的初始化问题。
附图说明
图1为本实施例的系统配置和多相机模型示意图；
图2为本实施例的概述流程图；
图3为本实施例的双针孔投影模型俯视图；
图4为本实施例的双针孔投影模型侧视图；
图5为本实施例的地图初始化方法中立体校正与匹配的结果图；
图6为本实施例的双向回环检测原理示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，故图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
下面结合实施例和附图对本发明进行详细说明。
如图2所示的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，摄像机位置分布呈现出一定的对称关系，所有摄像机均以10Hz的频率输出四个软件同步的1920×1208图像。
所述SLAM系统概述流程如图2所示，包括以下步骤：
(1)标定摄像机内部和外部参数，其中，本实施例采用全向摄像机模型对鱼眼相机进行建模，该模型使用多项式拟合鱼眼相机的成像映射函数，需要标定的内部参数包括多项式系数、畸变系数和图像中心坐标。采用大棋盘格法标定外部参数，使四个摄像机分别观测到棋盘格的一部分，再把主体坐标系设置在前置摄像机的相机坐标系，需要标定的外部参数包括从左置摄像机到前置摄像机、从右置摄像机到前置摄像机和从后置摄像机到前置摄像机的刚性变换。
多相机系统模型和主体坐标系的示意如图1；多相机系统模型引入主体坐标系，该坐标系是表示多相机系统绝对位姿的中间坐标系，既将每个摄像机的观测值单独分开，同时将所有观测值组合为一个观测方程式；多相机系统模型包含三种坐标系：世界坐标系(world)、主体坐标系(body)和相机坐标系(camera)，并且将坐标系标记在符号的右下角，其数学表达式如下：
Figure GDA0002527316840000071
其中，刚性变换T在三维空间R3中被参数化为轴角旋转矢量r和平移矢量t，它将3D点X转换为T*X＝R(r)X+t，R(r)为r的3×3旋转矩阵，*表示变换的组成，
Figure GDA0002527316840000081
表示在时刻t从主体坐标系b到第i个相机坐标系ci的刚性变换，
Figure GDA0002527316840000082
表示在时刻t从世界坐标系w到主体坐标系b的刚性变换，
Figure GDA0002527316840000083
分别表示在时刻t相机坐标系c和世界坐标系w下的一个点。
(2)当输入的四张鱼眼图像到达时，如果系统尚未进行初始化，首先利用双针孔投影模型和双目测距原理完成四目SLAM初始化，四目SLAM初始化利用摄相机外部参数结合双针孔投影模型，对视图间的重叠区域进行立体校正，通过立体匹配策略来执行视图间的特征匹配，然后对所有的匹配特征点对进行三角测量，生成初始地图。初始化过程中立体校正与匹配的结果如图5所示。
所述四张鱼眼图像的特征在于，它们是经过软件同步的同一时刻的四张图像，并且来自相邻摄像机的图像(如前置和左置、前置和右置、右置和后置、后置和左置)之间存在重叠区域。
所述双针孔投影模型原理示意图如图3、图4所示，双针孔投影模型包含两部分，每个部分都是独立的针孔模型，各自的成像平面相交形成一定的夹角，交线是投影平面的分界线，从而确保视图之间的重叠区域是透视的，并且在工程上易于实现，双针孔投影模型的数学表达式如下：
x＝KRpcXc,
其中，Xc为相机坐标系下的3D点，x为投影平面上的像素坐标，K为针孔相机的内参矩阵，Rpc为从相机坐标系到投影平面坐标系(plane)的3×3旋转矩阵。
(3)如果系统已经完成初始化，通过双针孔投影模型将输入图像校正为合适的双针孔透视图像，并且在每个视图的校正图像上提取特征。
(4)本实施例采用匀速模型或重定位估计初始的主体位姿，结合摄像机外部参数将地图点转换到每个视图进行特征跟踪和匹配。
所述特征跟踪和匹配的过程为：获取前一个多关键帧中对应的所有地图点，利用初始的主体位姿将这些地图点投影到当前多关键帧中每个视图的像平面，再根据设置的搜索半径在投影像素点附近确定搜索区域，取出搜索区域内的所有特征点作为候选匹配点集，计算地图点的描述子和候选匹配点集的描述子之间的距离，获得描述子距离最近且介于上阈值和下阈值之间的候选特征点作为最佳匹配，本实施例中使用的上阈值为100，下阈值为50。
(5)固定局部地图的地图点进行多视图姿态优化，更新估计的主体姿态。所述多视图姿态优化使用的代价函数为：
Figure GDA0002527316840000091
其中，
Figure GDA0002527316840000092
为从主体坐标系b到相机j坐标系cj的变换，
Figure GDA0002527316840000093
为相机j中的第i项，
Figure GDA0002527316840000094
为相机j中的第i个像素点，
Figure GDA0002527316840000095
为相机j中的第i个匹配地图点，π0(·)为双针孔投影模型，ρ为异常值影响最小化的柯西(Cauchy)损失函数，Tbw表示从世界坐标系w到主体坐标系b的刚性变换，|| ||2表示二范数的平方。
(6)完成多关键帧插入、新地图点生成和局部地图更新后，联合局部地图的所有位姿和地图点进行多视图BA优化。所述多视图BA优化使用的代价函数为：
Figure GDA0002527316840000096
其中，
Figure GDA0002527316840000097
表示t时刻相机j中的第i个像素点，
Figure GDA0002527316840000098
表示t时刻相机j中的第i个匹配地图点。
所述步骤(5)和步骤(6)中，对于优化任务，采用在g2o求解器中添加新的优化边，并提供解析形式的雅可比，通过整合多相机系统模型和双针孔投影模型，得到从世界点Xw到像素点x的最终方程为：
x＝KRpcTcb*Tbw*Xw,
其中，x为投影平面上的像素点，Xw为世界坐标系w下的3D点，K为针孔相机的内参矩阵，Rpc为从相机坐标系c到投影平面坐标系p的3×3旋转矩阵，Tcb为从主体坐标系b到相机坐标系c的刚性变换，Tbw为从世界坐标系w到主体坐标系b的刚性变换。
使用李代数
Figure GDA0002527316840000101
表示刚性变换T∈SE(3)，其中
Figure GDA0002527316840000102
为特殊欧式群SE(3)对应的李代数，φ为特殊正交群SO(3)对应的李代数，ρ为三维平移向量，上标T表示转置，并令观测误差e＝x-KRpcTcb*Tbw*Xw，则解析形式的雅可比为：
Figure GDA0002527316840000103
Figure GDA0002527316840000104
其中，x＝KXp，Xp＝RpcXc，Xc＝TcwXw，Tcw＝Tcb*Tbw，Tcw为从世界坐标系w到相机坐标系c的刚性变换，Tbw为从世界坐标系w到主体坐标系b的刚性变换，ε为Tcw对应的李代数，ξ为Tbw对应的李代数，Jξ为观测误差e对ξ的导数，
Figure GDA0002527316840000105
为观测误差e对Xw的导数，Xw为世界坐标系w下的3D点，Xp为投影平面坐标系p下的3D点，Xc为相机坐标系c下的3D点，
Figure GDA0002527316840000106
为Xc的斜对称矩阵，I3×3为3×3的单位阵，
Figure GDA0002527316840000107
为刚性变换Tcb∈SE(3)的伴随表示，
Figure GDA0002527316840000108
R为刚性变换T的旋转部分，τ为刚性变换T的平移部分，Rcb为刚性变换Tcb的旋转部分，Rbw为刚性变换Tbw的旋转部分。
(7)针对多视图的特点，采用双向对称的查询策略，保证正、反向经过同一场景时都能正确检测到回环，回环检测成功后进行回环校正。
所述回环检测是借助大视场和摄像机对称分布的天然优势，当前关键帧和待回环候选帧之间为相同或对称关系。双向回环检测原理参阅图6。汽车从A位置经过某个地方，使用左、右两个摄像机分别观测到左、右两侧的场景。当汽车从B位置反向经过同样的地方时，此时左置摄像机无法观测到左侧场景，但能观察到右侧场景，右置摄像机同理。此时，查询回环候选帧时，仅需搜索相同或对称位置的关键帧。
本发明所提出的基于多鱼眼摄像机和双针孔投影模型的SLAM方法能够解决现有视觉SLAM方法视野受限、尺度问题和实时性等问题，提高系统精度和鲁棒性，从而保证AVPS自主定位的充分性，具有高度产业价值。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，包括以下步骤：
步骤(1)标定摄像机内部和外部参数，其中摄像机内部参数是指标定所使用的鱼眼相机模型中涉及的所有参数，外部参数是指从摄像机坐标系到多相机系统模型中主体坐标系的刚性变换所涉及到的参数；
步骤(2)输入若干个鱼眼图像，如果系统尚未进行初始化，首先利用双针孔投影模型和双目测距原理完成多目SLAM初始化；
步骤(3)如果已经完成初始化，通过双针孔投影模型将输入图像校正为合适的双针孔透视图像，并且在每个视图的校正图像上提取特征；
步骤(4)利用运动模型或重定位估计初始的主体位姿，结合摄像机外部参数将地图点转换到每个视图进行特征跟踪和匹配；
步骤(5)固定局部地图的地图点进行多视图姿态优化，更新估计的主体姿态；
步骤(6)完成多关键帧插入、新地图点生成和局部地图更新后，联合局部地图的所有位姿和地图点进行多视图捆绑调整(bundle adjustment,BA)优化；
步骤(7)针对多视图的特点，采用双向对称的查询方式，保证正、反向经过同一场景时都能正确检测到回环，回环检测成功后进行回环校正；
在步骤(5)和步骤(6)中，对于优化任务，采用在g2o求解器中添加新的优化边，并提供解析形式的雅可比，通过整合多相机系统模型和双针孔投影模型，得到从世界点Xw到像素点x的最终方程为：
x＝KRpcTcbTbwXw；
其中，x为投影平面上的像素点，Xw为世界坐标系w下的3D点，K为针孔相机的内参矩阵，Rpc为从相机坐标系c到投影平面坐标系p的3×3旋转矩阵，Tcb为从主体坐标系b到相机坐标系c的刚性变换，Tbw为从世界坐标系w到主体坐标系b的刚性变换；
使用李代数
Figure FDA0004003851190000021
表示刚性变换T∈SE(3)，其中
Figure FDA0004003851190000022
为特殊欧式群SE(3)对应的李代数，φ为特殊正交群SO(3)对应的李代数，ρ为三维平移向量，上标T表示转置，并令观测误差e＝x-KRpcTcbTbwXw，则解析形式的雅可比为：
Figure FDA0004003851190000023
Figure FDA0004003851190000024
其中，x＝KXp，Xp＝RpcXc，Xc＝TcwXw，Tcw＝TcbTbw，Tcw为从世界坐标系w到相机坐标系c的刚性变换，Tbw为从世界坐标系w到主体坐标系b的刚性变换，∈为Tcw对应的李代数，ξ为Tbw对应的李代数，Jξ为观测误差e对ξ的导数，
Figure FDA0004003851190000025
为观测误差e对Xw的导数，Xw为世界坐标系w下的3D点，Xp为投影平面坐标系p下的3D点，Xc为相机坐标系c下的3D点，
Figure FDA0004003851190000026
为Xc的斜对称矩阵，I3×3为3×3的单位阵，
Figure FDA0004003851190000027
为刚性变换Tcb∈SE(3)的伴随表示，
Figure FDA0004003851190000028
R为刚性变换T的旋转部分，τ为刚性变换T的平移部分，Rcb为刚性变换Tcb的旋转部分，Rbw为刚性变换Tbw的旋转部分。
2.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，所述步骤(1)中，在多相机系统模型引入主体坐标系，主体坐标系是表示多相机系统绝对位姿的中间坐标系，既将每个摄像机的观测值单独分开，同时将所有观测值组合为一个观测方程式；多相机系统模型包含三种坐标系：世界坐标系(world)、主体坐标系(body)和相机坐标系(camera)，并且将坐标系标记在符号的右下角，其数学表达式如下：
Figure FDA0004003851190000029
其中，刚性变换T在三维空间R3中被参数化为轴角旋转矢量r和平移矢量t，它将3D点X转换为T*X＝R(r)X+t，R(r)为r的3×3旋转矩阵，*表示变换的组成，
Figure FDA00040038511900000210
表示在时刻t从主体坐标系b到第i个相机坐标系ci的刚性变换，
Figure FDA00040038511900000211
表示在时刻t从世界坐标系w到主体坐标系b的刚性变换，
Figure FDA00040038511900000212
分别表示在时刻t相机坐标系c和世界坐标系w下的一个点。
3.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，步骤(2)中，所述若干个鱼眼图像是经过时间同步的同一时刻的若干个图像，并且来自相邻摄像机的图像之间存在重叠区域。
4.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，步骤(2)中，所述双针孔投影模型包含两部分，每个部分都是独立的针孔模型，各自的成像平面相交形成一定的夹角，交线是投影平面的分界线，从而确保视图之间的重叠区域是透视的，并且在工程上易于实现，双针孔投影模型的数学表达式如下：
x＝KRpcXc，
其中，Xc为相机坐标系c下的3D点，x为投影平面上的像素坐标，K为针孔相机的内参矩阵，Rpc为从相机坐标系c到投影平面坐标系p的3×3旋转矩阵。
5.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，所述步骤(2)的初始化是利用摄相机外部参数结合双针孔投影模型，对视图间的重叠区域进行立体校正，通过立体匹配策略来执行视图间的特征匹配，然后对所有的匹配特征点对进行三角测量，生成初始地图。
6.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，所述步骤(4)中，所述特征跟踪和匹配的过程如下：
获取前一个多关键帧中对应的所有地图点，利用初始的主体位姿将这些地图点投影到当前多关键帧中每个视图的像平面，再根据设置的搜索半径在投影像素点附近确定搜索区域，取出搜索区域内的所有特征点作为候选匹配点集，计算地图点的描述子和候选匹配点集的描述子之间的距离，获得描述子距离最近且介于上阈值和下阈值之间的候选特征点作为最佳匹配。
7.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，所述步骤(5)中，所述多视图姿态优化使用的代价函数为：
Figure FDA0004003851190000031
其中，
Figure FDA0004003851190000041
为从主体坐标系b到相机j坐标系cj的变换，
Figure FDA0004003851190000042
为相机j中的第i项，
Figure FDA0004003851190000043
为相机j中的第i个像素点，
Figure FDA0004003851190000044
为相机j中的第i个匹配地图点，π0(·)为双针孔投影模型，ρ为异常值影响最小化的柯西(Cauchy)损失函数，Tbw表示从世界坐标系w到主体坐标系b的刚性变换，|| ||2表示二范数的平方。
8.根据权利要求7所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，所述步骤(6)中，多视图BA优化使用的代价函数为：
Figure FDA0004003851190000045
其中，
Figure FDA0004003851190000046
表示t时刻相机j中的第i个像素点，
Figure FDA0004003851190000047
表示t时刻相机j中的第i个匹配地图点。
9.根据权利要求1所述的一种基于多鱼眼摄像机和双针孔投影模型的SLAM方法，其特征在于，所述步骤(7)中，所述回环检测是借助大视场和摄像机对称分布的天然优势，当反向经过正向观测的同一地方时，由于观测角度刚好相反，原观测场景的摄像机无法再次观测，换成被与其对称分布的摄像机观测到，使得在当前关键帧和待回环候选帧上两者之间为相同或对称关系，因此查询回环候选帧时，仅需搜索相同或对称位置的关键帧。
CN202010132872.9A 2020-02-29 2020-02-29 一种基于多鱼眼摄像机和双针孔投影模型的slam方法 Active CN111415375B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010132872.9A CN111415375B (zh) 2020-02-29 2020-02-29 一种基于多鱼眼摄像机和双针孔投影模型的slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010132872.9A CN111415375B (zh) 2020-02-29 2020-02-29 一种基于多鱼眼摄像机和双针孔投影模型的slam方法

Publications (2)

Publication Number Publication Date
CN111415375A CN111415375A (zh) 2020-07-14
CN111415375B true CN111415375B (zh) 2023-03-21

Family

ID=71492808

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010132872.9A Active CN111415375B (zh) 2020-02-29 2020-02-29 一种基于多鱼眼摄像机和双针孔投影模型的slam方法

Country Status (1)

Country Link
CN (1) CN111415375B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112562052B (zh) * 2020-12-03 2021-07-27 广东工业大学 一种面向近岸水域的实时定位与建图方法
CN112734841B (zh) * 2020-12-31 2023-04-28 华南理工大学 一种用轮式里程计-imu和单目相机实现定位的方法
CN113516692A (zh) * 2021-05-18 2021-10-19 上海汽车集团股份有限公司 一种多传感器融合的slam方法和装置
CN113298014B (zh) * 2021-06-09 2021-12-17 安徽工程大学 基于逆向索引关键帧选取策略的闭环检测方法、存储介质及设备

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107610175A (zh) * 2017-08-04 2018-01-19 华南理工大学 基于半直接法和滑动窗口优化的单目视觉slam算法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107610175A (zh) * 2017-08-04 2018-01-19 华南理工大学 基于半直接法和滑动窗口优化的单目视觉slam算法
CN109509230A (zh) * 2018-11-13 2019-03-22 武汉大学 一种应用于多镜头组合式全景相机的slam方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
An Online Automatic Calibration Method Based on Feature Descriptor for Non-Overlapping Multi-Camera Systems;Long Zhang et al.;《2018 IEEE International Conference on Robotics and Biomimetics (ROBIO)》;第1-2页 *

Also Published As

Publication number Publication date
CN111415375A (zh) 2020-07-14

Similar Documents

Publication Publication Date Title
CN111415375B (zh) 一种基于多鱼眼摄像机和双针孔投影模型的slam方法
CN102005039B (zh) 基于泰勒级数模型的鱼眼相机立体视觉深度测量方法
US10469828B2 (en) Three-dimensional dense structure from motion with stereo vision
CN108534782B (zh) 一种基于双目视觉系统的地标地图车辆即时定位方法
CN107680159B (zh) 一种基于投影矩阵的空间非合作目标三维重建方法
WO2019029099A1 (zh) 基于图像梯度联合优化的双目视觉里程计算方法
CN104376552A (zh) 一种3d模型与二维图像的虚实配准算法
WO2019233090A1 (zh) 一种同时定位与建图的方法及装置
Chatterjee et al. Algorithms for coplanar camera calibration
CN112734839B (zh) 一种提高鲁棒性的单目视觉slam初始化方法
WO2024045632A1 (zh) 基于双目视觉和imu的水下场景三维重建方法及设备
Lagisetty et al. Object detection and obstacle avoidance for mobile robot using stereo camera
US20230351625A1 (en) A method for measuring the topography of an environment
CN102222348A (zh) 一种三维目标运动矢量计算方法
CN107330934B (zh) 低维度的集束调整计算方法与系统
CN111429571B (zh) 一种基于时空图像信息联合相关的快速立体匹配方法
CN117115271A (zh) 无人机飞行过程中的双目相机外参数自标定方法及系统
Das et al. Extrinsic calibration and verification of multiple non-overlapping field of view lidar sensors
CN116128966A (zh) 一种基于环境物体的语义定位方法
Guillemaut et al. Using points at infinity for parameter decoupling in camera calibration
CN107806861B (zh) 一种基于本质矩阵分解的倾斜影像相对定向方法
CN111145267A (zh) 基于imu辅助的360度全景视图多相机标定方法
Lu et al. Binocular stereo vision based on OpenCV
CN113048985B (zh) 已知相对旋转角度条件下的像机相对运动估计方法
Feng et al. Research on Calibration Method of Multi-camera System without Overlapping Fields of View Based on 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
GR01 Patent grant
GR01 Patent grant