CN111337947A - 即时建图与定位方法、装置、系统及存储介质 - Google Patents

即时建图与定位方法、装置、系统及存储介质 Download PDF

Info

Publication number
CN111337947A
CN111337947A CN202010416829.5A CN202010416829A CN111337947A CN 111337947 A CN111337947 A CN 111337947A CN 202010416829 A CN202010416829 A CN 202010416829A CN 111337947 A CN111337947 A CN 111337947A
Authority
CN
China
Prior art keywords
local
image
map
frame
point cloud
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.)
Granted
Application number
CN202010416829.5A
Other languages
English (en)
Other versions
CN111337947B (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.)
Shenzhen Smart Mapping Tech Co ltd
Original Assignee
Shenzhen Smart Mapping Tech Co ltd
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 Shenzhen Smart Mapping Tech Co ltd filed Critical Shenzhen Smart Mapping Tech Co ltd
Priority to CN202010416829.5A priority Critical patent/CN111337947B/zh
Publication of CN111337947A publication Critical patent/CN111337947A/zh
Application granted granted Critical
Publication of CN111337947B publication Critical patent/CN111337947B/zh
Priority to PCT/CN2021/087531 priority patent/WO2021233029A1/en
Priority to US17/624,317 priority patent/US20230260151A1/en
Priority to EP21728155.9A priority patent/EP3977346A1/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/60—Type of objects
    • G06V20/64—Three-dimensional objects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/213—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods
    • G06F18/2135—Feature extraction, e.g. by transforming the feature space; Summarisation; Mappings, e.g. subspace methods based on approximation criteria, e.g. principal component analysis
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/13—Edge detection
    • 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/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
    • G06V10/457—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components by analysing connectivity, e.g. edge linking, connected component analysis or slices
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/75—Organisation of the matching processes, e.g. simultaneous or sequential comparisons of image or video features; Coarse-fine approaches, e.g. multi-scale approaches; using context analysis; Selection of dictionaries
    • G06V10/757—Matching configurations of points or features
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes
    • 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/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • 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/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • 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/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging

Abstract

本发明实施例涉及一种即时建图与定位方法、装置、系统及存储介质，该方法包括：获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；分别对局部双目视觉图像和局部三维激光点云图像进行预处理；根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位。通过该方法，输出具有全局一致性的稠密三维点云图像，实时确定自身当前所在位置信息，完成即时建图和定位。

Description

即时建图与定位方法、装置、系统及存储介质
技术领域
本发明实施例涉及机器人技术领域，尤其涉及一种即时建图与定位方法、装置、系统及存储介质。
背景技术
智能机器人作为一个交叉学科，融合了运动动力学、系统结构、传感技术、控制技术等学科技术，而在这些技术中，提供移动机器人进行环境感知和定位是项必不可少的过程，通过环境感知与定位可反馈移动机器人当前的外部环境信息和自身在环境中的位置姿态信息，以进一步做相应的决策并控制电机前行。即时建图与定位作为移动智能机器人必不可少的底层技术，不仅需要给机器人提供即时的定位信息，还需要实时构建环境的稠密地图以供规划模块进行相应的决策并实现障碍物避障等功能。特别是在一些特定应用，如物流运输机器人、电力巡检机器人、园区载人自动驾驶小车等，高精度的定位信息如厘米级甚至毫米级别至关重要，只有具备高精度的定位信息，才能实现自主充电、定点巡查、定点停车等功能。
例如目前智能移动机器人都需具备一个最基础功能，即自主充电。在一般实践中，由于电池技术限制和以及巡查24小时不间断的需求，移动智能机器人持续作业实际中很难达到12小时以上，或者说无法连续持续作业，电池电量早晚会耗尽。在应用要求中需要移动机器人持续作业以保持功能的不间断，所以移动智能机器人的自主充电功能必不可少。但在实际情况中，充电房是一个密闭的小房间，充电插槽允许误差小，具有外部环境特征少、定位精度要求高的特点，所以高精度的即时定位需求非常迫切。
即时建图与定位（Simultaneous Localization and Mapping，简称SLAM）需要在估计自身在环境中的位置姿态的同时构建外部环境的地图信息。目前主流的方法有基于单线激光雷达SLAM、基于三维激光雷达SLAM、基于视觉惯性SLAM、基于纯视觉SLAM等等。
基于单线激光雷达SLAM依靠单线激光雷达所获取的传感器与外部环境的距离信息进行卡尔曼滤波估计，获取位置姿态等状态量信息的同时构建环境的平面栅格地图。由于该方法仅能估计平面信息和航向角，顾仅适用于室内非起伏的密闭环境中。基于三维激光雷达SLAM依靠多线激光雷达，通过每一帧激光雷达之间和每一帧与地图之间的关系构建非线性模型进行状态估计，获取6DOF的姿态和稠密的环境地图。由于三维激光雷达所获取的数据为离散的三维点，很难通过离散点进行闭环检测，从而导致误差大大增大，而且在结构化少的环境中如充电房，激光雷达SLAM在状态估计时很容易退化直至失败。基于视觉惯性SLAM和基于纯视觉SLAM都是以相机为基础传感器的技术方案，基于视觉的方法能够较容易通过纹理信息进行回环检测，且适用于结构化少的场景，但基于视觉的SLAM系统很难获取高精度的稠密地图，而稠密地图在移动智能机器人的规划决策中必不可少。
那么，如何才可以对位置姿态等状态量进行高精度的估计同时，还可以获取环境的稠密三维地图信息，成为本申请亟待解决的技术问题。
发明内容
鉴于此，为解决现有技术中上述技术问题，本发明实施例提供一种即时建图与定位方法、装置、系统及存储介质。
第一方面，本发明实施例提供一种即时建图与定位方法，该方法包括：
获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
分别对局部双目视觉图像和局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中全局地图为根据全局三维激光点云图像构建的全局地图。
在一个可能的实施方式中，对局部双目视觉图像进行预处理，具体包括：
对局部双目视觉图像进行特征提取，获取局部双目视觉图像的特征点；
从局部双目视觉图像的特征点中筛选出符合预设条件的特定特征点；
计算特定特征点的特征描述子；
根据特征描述子，对双目视觉图像进行特征匹配，以便后续根据经过特征匹配后获取的局部双目视觉图像，构建局部地图。
在一个可能的实施方式中，对局部三维激光点云图像进行预处理，具体包括：
从三维激光点云图像中提取地面点和非地面点；
并根据预设方法，区分地面点中的角点和平点；以及，区分非地面点中的角点和平点；
将地面点中的角点和非地面点中的角点共同构成角点集合，以及将地面点中的平点和非地面点中的平点共同构成平点集合。
在一个可能的实施方式中，根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿，具体包括：
根据经过预处理后的局部双目视觉图像，完成局部地图跟踪；
并在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
在一个可能的实施方式中，根据经过预处理后的局部双目视觉图像，完成具体地图跟踪，具体包括：
将经过处理后的局部双目视觉图像中首帧图像作为关键帧插入局部地图中；
以首帧图像作为参考帧，与新加入的当前帧图像进行特征点匹配；
根据首帧图像和当前帧图像之间相匹配的特征点，计算首帧图像和当前帧图像之间的位置之差和/或旋转角度之差；
当根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，将当前帧插入到局部地图中；
并与下一个新加入帧图像重复执行以上操作步骤，直至局部双目视觉图像中的所有帧图像处理完毕，完成对局部地图跟踪。
在一个可能的实施方式中，根据位置之差和/或旋转角度之差，确定当前帧为关键帧，具体包括：
当计算的是首帧图像和当前帧图像之间的位置之差，且位置之差大于或者等于第一预设阈值时，确定当前帧为关键帧；
或，当计算的是首帧图像和当前帧图像之间的旋转角度之差，且旋转角度之差大于或者等于第二预设阈值时，确定当前帧为关键帧；
或者，当计算的是首帧图像和当前帧图像之间的位置之差以及旋转角度之差，且位置之差和旋转角度之差均大于或者等于与之对应的预设阈值时，确定当前帧为关键帧。
在一个可能的实施方式中，在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿，具体包括：
当确定局部地图插入新的关键帧后，在当前已获取的局部地图中选择与新的关键帧满足共视关系的其他关键帧；
利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计；
当确定残差落入预设空间范围时，获取局部双目视觉地图位姿。
在一个可能的实施方式中，将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图，具体包括：
利用时间插值法，根据局部双目视觉地图位姿以及经过预处理后的局部三维激光点云图像，确定经过预处理后的局部三维激光点云图像中每一帧点云图像对应的位姿；
从经过预处理后的局部三维激光点云图像中提取预设帧数的点云图像，构建局部点云地图；
并将局部点云地图中首帧点云图像作为关键帧点云图像；
对新加入的一帧点云图像进行位姿优化；
并确定经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值；
当根据经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值，确定经过位姿优化后的点云图像为关键帧后，将经过位姿优化后的点云图像加入到局部点云地图中；
并以经过位姿优化后的点云图像作为当前关键帧，对下一帧新加入的图像执行重复执行以上操作步骤，直至所有点云图像执行以上操作步骤完成，获取局部融合地图。
在一个可能的实施方式中，对新加入的一帧点云图像进行位姿优化，具体包括：
从局部点云图像对应的角点集合中，查找与新加入的一帧点云图像中每一个角点分别对应的n个近邻角点，以及从局部点云图像对应的平点集合中，查找与新加入的一帧点云图像中每一个平点分别对应的j个近邻平点；
分别计算与第一角点对应的n个近邻角点的特征值，以及与第一平点对应的j个近邻平点的特征值；
当根据n个近邻角点分别对应的特征值，确定第一角点对应的临近点呈线状特征时，提取由n个近邻角点构成的直线中的第一预设数量点；
当根据j个近邻平点分别对应的特征值，确定第一平点对应的临近点呈面状特征时，提取由n个近邻平点构成的空间平面中的第二预设数量点；
根据第一预设数量点与其所在直线之间的对应关系、第二预设数量点与其所在空间平面之间的对应关系，以及新加入的一帧点云图像的位姿，构建代价函数；
利用代价函数，对新加入的一帧点云图像进行位姿优化，其中，第一角点为新加入的一帧点云图像中任一个角点，第一平点为新加入的一帧点云图像中任一平点，n和j均是正整数。
在一个可能的实施方式中，根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，具体包括：
利用激光闭环检测方法获取局部融合地图中第一帧融合图像对应的临近帧图像；
获取第一帧融合图像和临近帧图像之间的相对位姿；
根据局部融合地图中每一帧融合图像、与每一帧融合图像对应的临近帧图像，以及二者之间的相对位置，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中，第一帧融合图像为局部融合地图中的任一帧融合图像。
第二方面，本发明实施例提供一种即时建图与定位装置，该装置包括：
获取单元，用于获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
处理单元，用于分别对局部双目视觉图像和局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
融合单元，用于将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
地图优化单元，用于根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中全局地图为根据全局三维激光点云图像构建的全局地图。
第三方面，本发明实施例提供一种即时建图与定位系统，该系统包括：
至少一个处理器和存储器；
处理器用于执行存储器中存储的即时建图与定位程序，以实现如第一方面任一实施方式所介绍的即时建图与定位方法。
第四方面，本发明实施例提供一种计算机存储介质，该计算机存储介质存储有一个或者多个程序，一个或者多个程序可被如第三方面所介绍的即时建图与定位系统执行，如第一方面任一实施方式所介绍的即时建图与定位方法。
本发明实施例提供的一种即时建图与定位方法，获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像，然后对二者分别进行预处理。将经过预处理后的局部双目视觉图像和经过预处理后的局部三维激光点云图像进行融合，获取局部融合地图。局部融合地图实际上就是利用经过预处理后的局部双目视觉图像对经过预处理后的局部三维激光点云图像进行优化，也即是对局部地图进行优化。在获取经过局部优化后的融合地图后，再根据局部融合地图，对全局地图进行全局一致性优化，从而获取稠密点云地图。通过将双目视觉图像和三维激光点云图像进行融合，不仅能够克服三维激光在特征结构少的场景下容易失败的情况，实现对位置姿态等状态量进行高精度的估计同时，还可输出具有全局一致性的稠密三维点云图像。而且，还可以根据稠密三维点云图像，实时确定自身当前所在位置信息，完成即时建图和定位。
附图说明
图1为本发明实施例提供的一种即时建图与定位方法流程示意图；
图2为本发明提供的一种双目视差原理图；
图3为本发明提供的一种利用时间差值法，获取局部三维激光点云图像中某一帧点云图像对应的位姿的方法示意图；
图4为本发明提供一种由激光关键帧所构建的激光位姿图；
图5为本发明实施例提供的一种即时建图与定位装置结构示意图；
图6为本发明实施例提供的一种即时建图与定位系统结构示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
为便于对本发明实施例的理解，下面将结合附图以具体实施例做进一步的解释说明，实施例并不构成对本发明实施例的限定。
图1为本发明实施例提供的一种即时建图与定位方法流程示意图，如图1所示，在本实施例中，以机器人处于陌生环境即时构建地图和定位的应用场景为例进行说明，该方法包括：
步骤110，获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像。
具体的，机器人利用图像采集装置对当前所处环境进行图像采集，获取双目视觉图像，同时利用三维激光雷达对当前位置所在的周围环境进行扫描，获取三维激光点云图像。
步骤120，分别对局部双目视觉图像和局部三维激光点云图像进行预处理。
可选的，对局部双目视觉图像进行预处理，可以包括如下步骤：
对局部双目视觉图像进行特征提取，获取局部双目视觉图像的特征点；
从局部双目视觉图像的特征点中筛选出符合预设条件的特定特征点；
计算特定特征点的特征描述子；
根据特征描述子，对双目视觉图像进行特征匹配，以便后续根据经过特征匹配后获取的局部双目视觉图像，构建局部地图。
具体的，在对局部双目视觉图像进行特征提取时，可以进行特征粗提取。例如，通过简单的判断图像像素点周围的灰度值变化，确定大量特征点。比如采用灰度图像特征提取方法提取图像特征。
根据上述方式，提取的图像特征点数量将会比较大。因此，还需要对特征点进行筛选，筛选出符合预设条件的特定特征点。
具体的，可以根据特征点周围预设数量的像素生成一个决策树，然后筛选出符合决策树要求的特征点，接着使用非极大值抑制法去除局部较密集特征点，剩余的特征点作为特定特征点。
最后为了顾及特征尺度变形性和旋转不变性，可以通过建立金字塔和使用矩阵法等对特征点进行计算，获取特征的尺度信息和旋转信息，作为特定特征点的特征描述子。众所周知，特征描述子是为了后续执行图像匹配时使用。即，根据特征描述子，对双目视觉图像进行特征匹配，以便后续根据经过特征匹配后获取的局部双目视觉图像，构建局部地图。
可选的，对局部三维激光点云图像进行预处理，具体包括：
从三维激光点云图像中提取地面点和非地面点；
并根据预设方法，区分地面点中的角点和平点；以及，区分非地面点中的角点和平点；
将地面点中的角点和非地面点中的角点共同构成角点集合，以及将地面点中的平点和非地面点中的平点共同构成平点集合。
在一个具体的例子中，三维激光雷达所获取的数据为三维点云，首先将激光雷达按照扫描方向进行投影，通过计算相邻深度提取激光雷达中的地面点和非地面点，然后计算扫描线上曲率值，认为曲率大的点为角点，曲率小的点为平点，最后将地面点和非地面点上的角点和平点进行汇总得到最终两组特征点集合，分别为角点集合和平点集合。
步骤130，根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿。
具体的，可以根据经过预处理后的局部双目视觉图像，完成局部地图跟踪；
并在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
可选的，根据经过预处理后的局部双目视觉图像，完成具体地图跟踪，可以通过如下方式实现：
将经过处理后的局部双目视觉图像中首帧图像作为关键帧插入局部地图中；
以首帧图像作为参考帧，与新加入的当前帧图像进行特征点匹配；
根据首帧图像和当前帧图像之间相匹配的特征点，计算首帧图像和当前帧图像之间的位置之差和/或旋转角度之差；
当根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，将当前帧插入到局部地图中；
并与下一个新加入帧图像重复执行以上操作步骤，直至局部双目视觉图像中的所有帧图像处理完毕，完成对局部地图跟踪。
具体的，在最开始的时候，将经过处理后的局部双目视觉图像中首帧图像作为关键帧，插入到局部地图中，也即是作为局部地图的初始帧。以该帧图像为起始，陆续从经过处理后的局部双目视觉图像中筛选出符合要求的帧图像，加入到局部地图中。在一个具体的例子中，局部地图由关键帧构成，那么就是要判断后续新加入的帧图像是否可以作为关键帧。
具体实现过程包括，以首帧图像为参考帧，与新加入的当前帧图像进行特征点匹配。
具体进行特征点匹配的过程已经在上文中进行说明，这里不再赘述。
然后，根据首帧图像和当前帧图像之间相匹配的特征点，采用PnP算法分别估计当前帧的位姿以及关键帧（当前来说就是首帧图像）的位姿。最终当前帧的位姿以及首帧图像的位姿，确定首帧图像和当前帧图像之间的位置之差和/或旋转角度之差。
并在根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，将当前帧插入到局部地图中。
具体的，在利用，在根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，可以采用如下判断方式：
当计算的是首帧图像和当前帧图像之间的位置之差，且位置之差大于或者等于第一预设阈值时，确定当前帧为关键帧；
或，当计算的是首帧图像和当前帧图像之间的旋转角度之差，且旋转角度之差大于或者等于第二预设阈值时，确定当前帧为关键帧；
或者，当计算的是首帧图像和当前帧图像之间的位置之差以及旋转角度之差，且位置之差和旋转角度之差均大于或者等于与之对应的预设阈值时，确定当前帧为关键帧。
上述仅仅是列举了通过首帧图像，确定下一帧图像是否是关键帧方式，再下一次执行时，可以以当前关键帧作为参考系，然后利用上面所介绍的方式进一步确定下一帧新加入的图像是否为关键帧。如果是则加入到局部地图中，由此构建局部地图，完成局部地图跟踪。
可选的，在根据PnP算法分别估计当前帧的位姿以及关键帧（当前来说就是首帧图像）的位姿时，还需要对具有特征匹配点的两帧图像（也即是当前帧和首帧图像）中所有左右目特征匹配点分别进行初始化处理，实现对每一帧图像的位姿进行优化。
在一个具体的例子中，双目视觉初始化可利用双目视差图对初始帧进行深度估 计。对双目视觉进行立体标定，立体标定工作可在前期标定工作中进行，进而获取如图2所 示的双目视差原理图。以某一个空间坐标点为为例，对其对应的具有特征匹配点的两帧图 像中，与该空间坐标点对应的左右目特征匹配点进行初始化处理为例，进行说明。具体参见 图2所示，图中
Figure 85688DEST_PATH_IMAGE001
与
Figure 678343DEST_PATH_IMAGE002
分别为图像采集装置左右目光心，例如图像采集装置都是相机，那么 就是左右目的相机光心，
Figure 103639DEST_PATH_IMAGE003
与
Figure 516166DEST_PATH_IMAGE004
为左右目特征匹配点，设两像素配点所对应 的空间坐标为
Figure 809744DEST_PATH_IMAGE005
。通过双目图像原理，可以得到空间坐标P
Figure 256906DEST_PATH_IMAGE006
。例如，空 间点的深度值
Figure 102371DEST_PATH_IMAGE007
可以用如下公式表示：
Figure 267773DEST_PATH_IMAGE008
（公式1）
其中，b为双目基线，f为焦距，
Figure 99463DEST_PATH_IMAGE009
和
Figure 401131DEST_PATH_IMAGE010
分别为左右目特征匹配点的横坐标值。
假设
Figure 168230DEST_PATH_IMAGE011
分别为任一帧图像对应的横坐标和纵坐标；P
Figure 555349DEST_PATH_IMAGE012
为与该帧图 像对应的空间点坐标。设K为相机内参参数，
Figure 190730DEST_PATH_IMAGE013
为像素深度值，相机的位姿为T=[R,t]，对应 的李代数为
Figure 612484DEST_PATH_IMAGE014
， 则有公式2，具体参见如下：
Figure 65331DEST_PATH_IMAGE015
Kexp(
Figure 674167DEST_PATH_IMAGE016
)
Figure 847659DEST_PATH_IMAGE017
（公式2）
公式2表示的状态为理想状态，表示为根据相机采集的图像特征点（公式左侧表达式）计算得到的位姿和根据实际空间坐标（公式右侧表达式）计算得到的位姿是相同的。也即是公式左侧表达式和公式右侧表达式应该相等。
那么，在理想状态中，也应该有当
Figure 123920DEST_PATH_IMAGE003
分别为首帧图像对应的横坐标和纵坐 标，
Figure 232821DEST_PATH_IMAGE004
分别为当前新加入的帧图像对应的横纵坐标；P
Figure 594532DEST_PATH_IMAGE006
为空间点坐标。设 K为相机内参参数，
Figure 571716DEST_PATH_IMAGE018
为像素深度值，相机的位姿为T=[R,t]，对应的李代数为
Figure 968062DEST_PATH_IMAGE014
时，将公式2 进行相应的应用后，参见公式3和公式4：
Figure 497132DEST_PATH_IMAGE019
Kexp(
Figure 80560DEST_PATH_IMAGE016
)
Figure 861434DEST_PATH_IMAGE020
（公式3）
Figure 112287DEST_PATH_IMAGE021
Kexp(
Figure 562991DEST_PATH_IMAGE016
)
Figure 633715DEST_PATH_IMAGE020
（公式4）
但是在实际应用过程中，由于通过图像采集装置采集到的数据计算位置必然存在一定的误差。因此，公式左右两侧必然是不相等的。为了让通过像素得到的位姿尽量接近实际位置，则可以通过最小二乘算法求取，让根据像素坐标得到的位姿和根据实际空间坐标得到的位姿误差最小。
具体实现时，可以假设共具有n对匹配点，基于n对匹配点构建关于相机位姿的代价函数，利用最小二乘进行求解。
具体参见公式如下：
Figure 952701DEST_PATH_IMAGE022
（公式5）
其中，
Figure 58061DEST_PATH_IMAGE023
为n对左右目特征点中第i对特征点中的其中一个特征点坐标，
Figure 197442DEST_PATH_IMAGE024
与第i对特 征点坐标对应的实际空间坐标，K为相机参数，
Figure 21042DEST_PATH_IMAGE014
为李代数。
Figure 878139DEST_PATH_IMAGE025
可以根据实际情况设定，通过 该公式求取
Figure 978951DEST_PATH_IMAGE014
。而后，其他特征点对只要满足该公式要求即可。
通过该公式，可以实现对对具有特征匹配点的两帧图像（也即是当前帧和首帧图像）中所有左右目特征匹配点分别进行初始化处理。从而实现对每一帧图像的位姿进行优化处理。以便在计算具有相同特征点的两帧图像之间的位置之差和/或旋转角度之差时能够更加精确。
通过上述步骤，可以实现局部地图由关键帧组成，即完成局部地图跟踪。但是，由于局部地图跟踪不可避免出现误差越来越大的问题，特别是当跟踪时间比较长时，误差会从上一帧累积并传递到当前帧直至误差越来越大，造成跟踪失败。
因此，可选的，在局部地图跟踪过程中，还包括对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
在具体实现时，可以通过如下方式：
当确定局部地图插入新的关键帧后，在当前已获取的局部地图中选择与新的关键帧满足共视关系的其他关键帧；
利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计；
当确定残差落入预设空间范围时，获取局部双目视觉地图位姿。
具体的，首先当局部地图插入新的关键帧后，选择局部地图中与此关键帧满足共视关系的其他关键帧组成局部优化地图，共视关系由两帧之间特征点匹配的个数决定，即两帧匹配的特征点数目要大于预设数量阈值。有了局部优化地图后，即可利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计，将误差约束在一定的空间范围之内。也即是，当确定残差落入预设空间范围时，获取局部双目视觉地图位姿。
局部光束法平差可根据PnP算法构建一个最小化重投影误差的非线性模型，相应的代价函数与公式(5)相同或者类似，这里不再过多赘述。通过上述方式，完成局部视觉跟踪和局部地图构建，获取局部双目视觉地图位姿。
步骤140，将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图。
具体的，通过视觉跟踪和局部地图构建，获取的局部双目视觉地图位姿可以作为激光雷达匹配的初始值进一步进行优化。一般而言相机一般的输出频率为30Hz，而三维激光雷达输出的频率一般为10Hz，因此，可利用时间进行插值获取三维激光雷达的位姿。
可选的，该步骤可以通过如下方式实现：首先，利用时间插值法，根据局部双目视觉地图位姿以及经过预处理后的局部三维激光点云图像，确定经过预处理后的局部三维激光点云图像中每一帧点云图像对应的位姿。
从经过预处理后的局部三维激光点云图像中提取预设帧数的点云图像，构建局部点云地图。
并将局部点云地图中首帧点云图像作为关键帧点云图像。
对新加入的一帧点云图像进行位姿优化。
并确定经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值。
当根据经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值，确定经过位姿优化后的点云图像为关键帧后，将经过位姿优化后的点云图像加入到局部点云地图中。
并以经过位姿优化后的点云图像作为当前关键帧，对下一帧新加入的图像执行重复执行以上操作步骤，直至所有点云图像执行以上操作步骤完成，获取局部融合地图。
具体的，具体参见图3所示，图3示出了利用时间差值法，获取局部三维激光点云图像中某一帧点云图像对应的位姿的方法示意图。
设当前激光帧为
Figure 896091DEST_PATH_IMAGE026
，对应的时间戳为
Figure 941408DEST_PATH_IMAGE027
，在视觉帧序列中，
Figure 602196DEST_PATH_IMAGE026
的时间大于视觉帧 序列的最近帧为
Figure 416568DEST_PATH_IMAGE028
，小于视觉帧序列最近帧为
Figure 629244DEST_PATH_IMAGE029
，通过以上步骤可获取
Figure 427435DEST_PATH_IMAGE028
与
Figure 891915DEST_PATH_IMAGE029
的姿 态信息分别为
Figure 701739DEST_PATH_IMAGE030
和
Figure 960682DEST_PATH_IMAGE031
，位置信息分别为：
Figure 980591DEST_PATH_IMAGE032
和
Figure 248761DEST_PATH_IMAGE033
。则通过时间插值可获取激光帧
Figure 162359DEST_PATH_IMAGE026
的 位姿。具体参见如下公式：
Figure 592203DEST_PATH_IMAGE034
（公式6）
Figure 99408DEST_PATH_IMAGE035
（公式7）
其中，
Figure 905690DEST_PATH_IMAGE036
为激光帧
Figure 424527DEST_PATH_IMAGE026
的姿态，
Figure 25273DEST_PATH_IMAGE037
为激光帧
Figure 19774DEST_PATH_IMAGE026
的位置，
Figure 629747DEST_PATH_IMAGE038
为视觉帧序列
Figure 252358DEST_PATH_IMAGE029
对应 的时间戳，
Figure 289584DEST_PATH_IMAGE039
为视觉帧序列
Figure 505802DEST_PATH_IMAGE028
对应的时间戳。
以上获取的激光帧位姿误差比较大，需要利用激光雷达的特征进一步优化。因此，还需要执行如下步骤：
首先维护一个局部点云地图，将起始前h帧作为关键帧加入局部地图，当新来一帧后，利用当前帧点云寻找与局部地图中点云的对应关系，通过点的对应关系构建一个最小二乘问题对位姿进行优化，然后判断当前帧与最新关键帧之间的位置和姿态差值，当差值小于设定的阈值时，判断当前帧为非关键帧，反之则为关键帧，最后如果当前帧为关键帧则将当前帧的点云插入局部地图中。
可选的，对新加入的一帧点云图像进行位姿优化，具体包括：
从局部点云图像对应的角点集合中，查找与新加入的一帧点云图像中每一个角点分别对应的n个近邻角点，以及从局部点云图像对应的平点集合中，查找与新加入的一帧点云图像中每一个平点分别对应的j个近邻平点；
分别计算与第一角点对应的n个近邻角点的特征值，以及与第一平点对应的j个近邻平点的特征值；
当根据n个近邻角点分别对应的特征值，确定第一角点对应的临近点呈线状特征时，提取由n个近邻角点构成的直线中的第一预设数量点；
当根据j个近邻平点分别对应的特征值，确定第一平点对应的临近点呈面状特征时，提取由n个近邻平点构成的空间平面中的第二预设数量点；
根据第一预设数量点与其所在直线之间的对应关系、第二预设数量点与其所在空间平面之间的对应关系，以及新加入的一帧点云图像的位姿，构建代价函数；
利用代价函数，对新加入的一帧点云图像进行位姿优化，其中，第一角点为新加入的一帧点云图像中任一个角点，第一平点为新加入的一帧点云图像中任一平点，n和j均是正整数。
在具体执行时，首先对当前帧角点集合中的每一个点
Figure 653886DEST_PATH_IMAGE040
在局部地图角点集 合
Figure 147316DEST_PATH_IMAGE041
中寻找最近邻
Figure 355443DEST_PATH_IMAGE042
个点，利用主成分分析PCA计算n个的特征值，当最大的特征值 远大于其他两个特征值时，认为
Figure 58957DEST_PATH_IMAGE040
临近点呈线状特征，取由
Figure 10732DEST_PATH_IMAGE042
个点所组成的空间直线 上的两个点分别是
Figure 599147DEST_PATH_IMAGE043
和
Figure 978176DEST_PATH_IMAGE044
；同理对当前帧平点集合中的每一个点
Figure 168985DEST_PATH_IMAGE045
在局部 地图平点集合
Figure 658873DEST_PATH_IMAGE046
中同样寻找最近临j个点，利用PCA计算特征，当最大与第二大特征 值接近且远大于最小特征时，认为临近点
Figure 126894DEST_PATH_IMAGE045
呈面状特征，取
Figure 411245DEST_PATH_IMAGE047
个点所组成的空间平面 上的三个点分别是
Figure 89351DEST_PATH_IMAGE048
、
Figure 382929DEST_PATH_IMAGE049
和
Figure 954725DEST_PATH_IMAGE050
。根据以上点与线、点与面的一一对应关系，可根 据当前帧位姿作为状态量构建一个非线性优化模型。设当前帧位姿对应的李代数为
Figure 675556DEST_PATH_IMAGE051
，则 可构建代价函数：
Figure 575379DEST_PATH_IMAGE052
（公式8）
以上代价函数分别表示点到对应点组成线段的距离以及点到对应点组成面的距离。通过构建如公式6所示的代价函数，利用利用L-M方法即可求得当前帧的最优姿态。从而实现对新加入的一帧点云图像进行位姿优化。
步骤150，根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位。
具体的，在步骤140中，即时加入了局部地图做局部优化，但也只考虑了局部特征，当地图构建足够长时，误差累计会越来越大，在第二次到达同一个位置时，由于误差的累计，相同的位置的位姿会差别很大，造成地图错误，因此需要通过闭环检测进行一致性优化。
可选的，具体实现方式可以包括：
利用激光闭环检测方法获取局部融合地图中第一帧融合图像对应的临近帧图像；
获取第一帧融合图像和临近帧图像之间的相对位姿；
根据局部融合地图中每一帧融合图像、与每一帧融合图像对应的临近帧图像，以及二者之间的相对位置，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中，第一帧融合图像为局部融合地图中的任一帧融合图像。
具体的，激光地图为离散的三维点，很难通过离散的信息进行相似位置的判断。但是可利用视觉，通过构建词袋模型，判断当前位置与历史位置是否为临近的位置，当判断为同一位置后，对两帧图像进行特征匹配，然后利用PnP方法构建如公式(5)所示的代价函数，获取两帧之间的相对位姿，接着将此求得的相对位姿作为该两帧图像所对应的激光帧的相对位姿的初始值，对两帧激光点云所对应的局部地图进行匹配，采用类似公式(6)即可优化该相对位姿得到最优估计值，最后将该相对位姿代入激光关键帧所构建的全局位姿图中进行整体优化，获得具有全局一致性的稠密点云地图。
具体参见图4所示，图4为由激光关键帧所构建的激光位姿图，各临近关键帧之间 所组成的边为两帧之间的相对姿态
Figure 407069DEST_PATH_IMAGE053
，假设当前帧为
Figure 849683DEST_PATH_IMAGE054
，通过以上闭环检测可检测到其 与激光帧
Figure 741415DEST_PATH_IMAGE055
临近，并通过类似公式5的计算可获取两帧之间的相对位姿
Figure 128534DEST_PATH_IMAGE056
，以每一帧的位 姿作为状态量
Figure 763915DEST_PATH_IMAGE057
，如帧
Figure 310303DEST_PATH_IMAGE055
的位姿为
Figure 372937DEST_PATH_IMAGE058
，则可构建代价函数对状态进 行最优估计，具体参见如下公式：
Figure 247352DEST_PATH_IMAGE059
（公式9）
通过以上步骤即可得到全局一致性的稠密点云地图，并实时输出当前的位置和姿态信息，完成即时定位与建图。
本发明实施例提供的即时建图与定位方法，获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像，然后对二者分别进行预处理。将经过预处理后的局部双目视觉图像和经过预处理后的局部三维激光点云图像进行融合，获取局部融合地图。局部融合地图实际上就是利用经过预处理后的局部双目视觉图像对经过预处理后的局部三维激光点云图像进行优化，也即是对局部地图进行优化。在获取经过局部优化后的融合地图后，再根据局部融合地图，对全局地图进行全局一致性优化，从而获取稠密点云地图。通过将双目视觉图像和三维激光点云图像进行融合，不仅能够克服三维激光在特征结构少的场景下容易失败的情况，实现对位置姿态等状态量进行高精度的估计同时，还可输出具有全局一致性的稠密三维点云图像。在此过程中，由于根据时间插值的轨迹融合方法，通过利用时间差与姿态四元数的特性实现了融合视觉轨迹与激光雷达轨迹。而且，通过闭环检测，克服了激光雷达由于只具备三维离散点而难以进行全局相似检测的问题，具有鲁棒性。最终，还可以根据稠密三维点云图像，实时确定自身当前所在位置信息，完成即时建图和定位。
图5为本发明实施例提供的一种即时建图与定位装置，该装置包括：获取单元501、处理单元502、融合单元503以及地图优化单元504。
获取单元501，用于获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
处理单元502，用于分别对局部双目视觉图像和局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
融合单元503，用于将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
地图优化单元504，用于根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中全局地图为根据全局三维激光点云图像构建的全局地图。
可选的，处理单元502具体用于，对局部双目视觉图像进行特征提取，获取局部双目视觉图像的特征点；
从局部双目视觉图像的特征点中筛选出符合预设条件的特定特征点；
计算特定特征点的特征描述子；
根据特征描述子，对双目视觉图像进行特征匹配，以便后续根据经过特征匹配后获取的局部双目视觉图像，构建局部地图。
可选的，处理单元502具体用于，从三维激光点云图像中提取地面点和非地面点；
并根据预设方法，区分地面点中的角点和平点；以及，区分非地面点中的角点和平点；
将地面点中的角点和非地面点中的角点共同构成角点集合，以及将地面点中的平点和非地面点中的平点共同构成平点集合。
可选的，处理单元502具体用于，根据经过预处理后的局部双目视觉图像，完成局部地图跟踪；
并在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
可选的，处理单元502具体用于，将经过处理后的局部双目视觉图像中首帧图像作为关键帧插入局部地图中；
以首帧图像作为参考帧，与新加入的当前帧图像进行特征点匹配；
根据首帧图像和当前帧图像之间相匹配的特征点，计算首帧图像和当前帧图像之间的位置之差和/或旋转角度之差；
当根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，将当前帧插入到局部地图中；
并与下一个新加入帧图像重复执行以上操作步骤，直至局部双目视觉图像中的所有帧图像处理完毕，完成对局部地图跟踪。
可选的，处理单元502具体用于，当计算的是首帧图像和当前帧图像之间的位置之差，且位置之差大于或者等于第一预设阈值时，确定当前帧为关键帧；
或，当计算的是首帧图像和当前帧图像之间的旋转角度之差，且旋转角度之差大于或者等于第二预设阈值时，确定当前帧为关键帧；
或者，当计算的是首帧图像和当前帧图像之间的位置之差以及旋转角度之差，且位置之差和旋转角度之差均大于或者等于与之对应的预设阈值时，确定当前帧为关键帧。
可选的，处理单元502具体用于，当确定局部地图插入新的关键帧后，在当前已获取的局部地图中选择与新的关键帧满足共视关系的其他关键帧；
利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计；
当确定残差落入预设空间范围时，获取局部双目视觉地图位姿。
可选的，融合单元503具体用于，利用时间插值法，根据局部双目视觉地图位姿以及经过预处理后的局部三维激光点云图像，确定经过预处理后的局部三维激光点云图像中每一帧点云图像对应的位姿；
从经过预处理后的局部三维激光点云图像中提取预设帧数的点云图像，构建局部点云地图；
并将局部点云地图中首帧点云图像作为关键帧点云图像；
地图优化单元504还用于，对新加入的一帧点云图像进行位姿优化；
处理单元502还拥有，确定经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值；
当根据经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值，确定经过位姿优化后的点云图像为关键帧后，将经过位姿优化后的点云图像加入到局部点云地图中；
并以经过位姿优化后的点云图像作为当前关键帧，对下一帧新加入的图像执行重复执行以上操作步骤，直至所有点云图像执行以上操作步骤完成，获取局部融合地图。
可选的，地图优化单元504还用于，从局部点云图像对应的角点集合中，查找与新加入的一帧点云图像中每一个角点分别对应的n个近邻角点，以及从局部点云图像对应的平点集合中，查找与新加入的一帧点云图像中每一个平点分别对应的j个近邻平点；
分别计算与第一角点对应的n个近邻角点的特征值，以及与第一平点对应的j个近邻平点的特征值；
当根据n个近邻角点分别对应的特征值，确定第一角点对应的临近点呈线状特征时，提取由n个近邻角点构成的直线中的第一预设数量点；
当根据j个近邻平点分别对应的特征值，确定第一平点对应的临近点呈面状特征时，提取由n个近邻平点构成的空间平面中的第二预设数量点；
根据第一预设数量点与其所在直线之间的对应关系、第二预设数量点与其所在空间平面之间的对应关系，以及新加入的一帧点云图像的位姿，构建代价函数；
利用代价函数，对新加入的一帧点云图像进行位姿优化，其中，第一角点为新加入的一帧点云图像中任一个角点，第一平点为新加入的一帧点云图像中任一平点，n和j均是正整数。
可选的，地图优化单元504具体用于，利用激光闭环检测方法获取局部融合地图中第一帧融合图像对应的临近帧图像；
获取第一帧融合图像和临近帧图像之间的相对位姿；
根据局部融合地图中每一帧融合图像、与每一帧融合图像对应的临近帧图像，以及二者之间的相对位置，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中，第一帧融合图像为局部融合地图中的任一帧融合图像。
本实施例提供的即时建图与定位装置中各功能部件所执行的功能均已在图1对应的实施例中做了详细介绍，因此这里不再赘述。
本发明实施例提供的一种即时建图与定位装置，获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像，然后对二者分别进行预处理。将经过预处理后的局部双目视觉图像和经过预处理后的局部三维激光点云图像进行融合，获取局部融合地图。局部融合地图实际上就是利用经过预处理后的局部双目视觉图像对经过预处理后的局部三维激光点云图像进行优化，也即是对局部地图进行优化。在获取经过局部优化后的融合地图后，再根据局部融合地图，对全局地图进行全局一致性优化，从而获取稠密点云地图。通过将双目视觉图像和三维激光点云图像进行融合，不仅能够克服三维激光在特征结构少的场景下容易失败的情况，实现对位置姿态等状态量进行高精度的估计同时，还可输出具有全局一致性的稠密三维点云图像。在此过程中，由于根据时间插值的轨迹融合方法，通过利用时间差与姿态四元数的特性实现了融合视觉轨迹与激光雷达轨迹。而且，通过闭环检测，克服了激光雷达由于只具备三维离散点而难以进行全局相似检测的问题，具有鲁棒性。最终，还可以根据稠密三维点云图像，实时确定自身当前所在位置信息，完成即时建图和定位。
图6为本发明实施例提供的一种即时建图与定位系统的结构示意图，图6所示的即时建图与定位系统600包括：至少一个处理器601、存储器602、至少一个网络接口603和其他用户接口604。即时建图与定位即时建图与定位系统600中的各个组件通过总线系统605耦合在一起。可理解，总线系统605用于实现这些组件之间的连接通信。总线系统605除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图6中将各种总线都标为总线系统605。
其中，用户接口604可以包括显示器、键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。
可以理解，本发明实施例中的存储器602可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(Read-Only Memory，ROM)、可编程只读存储器 (Programmable ROM，PROM)、可擦除可编程只读存储器(Erasable PROM，EPROM)、电可擦除可编程只读存储器(Electrically EPROM，EEPROM)或闪存。易失性存储器可以是随机存取存储器(Random Access Memory，RAM)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器(Static RAM，SRAM)、动态随机存取存储器 (Dynamic RAM，DRAM)、同步动态随机存取存储器(Synchronous DRAM，SDRAM)、双倍数据速率同步动态随机存取存储器(Double DataRate SDRAM，DDRSDRAM)、增强型同步动态随机存取存储器(Enhanced SDRAM，ESDRAM)、同步连接动态随机存取存储器(Synch link DRAM，SLDRAM)和直接内存总线随机存取存储器(Direct Rambus RAM，DRRAM)。 本文描述的存储器602旨在包括但不限于这些和任意其它适合类型的存储器。
在一些实施方式中，存储器602存储了如下的元素，可执行单元或者数据结构，或者他们的子集，或者他们的扩展集：操作系统6021和应用程序 6022。
其中，操作系统6021，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序6022，包含各种应用程序，例如媒体播放器(Media Player)、浏览器(Browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序6022中。
在本发明实施例中，通过调用存储器602存储的程序或指令，具体的，可以是应用程序6022中存储的程序或指令，处理器601用于执行各方法实施例所提供的方法步骤，例如包括：
获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
分别对局部双目视觉图像和局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中全局地图为根据全局三维激光点云图像构建的全局地图。
可选的，对局部双目视觉图像进行特征提取，获取局部双目视觉图像的特征点；
从局部双目视觉图像的特征点中筛选出符合预设条件的特定特征点；
计算特定特征点的特征描述子；
根据特征描述子，对双目视觉图像进行特征匹配，以便后续根据经过特征匹配后获取的局部双目视觉图像，构建局部地图。
可选的，从三维激光点云图像中提取地面点和非地面点；
并根据预设方法，区分地面点中的角点和平点；以及，区分非地面点中的角点和平点；
将地面点中的角点和非地面点中的角点共同构成角点集合，以及将地面点中的平点和非地面点中的平点共同构成平点集合。
可选的，根据经过预处理后的局部双目视觉图像，完成局部地图跟踪；
并在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
可选的，将经过处理后的局部双目视觉图像中首帧图像作为关键帧插入局部地图中；
以首帧图像作为参考帧，与新加入的当前帧图像进行特征点匹配；
根据首帧图像和当前帧图像之间相匹配的特征点，计算首帧图像和当前帧图像之间的位置之差和/或旋转角度之差；
当根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，将当前帧插入到局部地图中；
并与下一个新加入帧图像重复执行以上操作步骤，直至局部双目视觉图像中的所有帧图像处理完毕，完成对局部地图跟踪。
可选的，当计算的是首帧图像和当前帧图像之间的位置之差，且位置之差大于或者等于第一预设阈值时，确定当前帧为关键帧；
或，当计算的是首帧图像和当前帧图像之间的旋转角度之差，且旋转角度之差大于或者等于第二预设阈值时，确定当前帧为关键帧；
或者，当计算的是首帧图像和当前帧图像之间的位置之差以及旋转角度之差，且位置之差和旋转角度之差均大于或者等于与之对应的预设阈值时，确定当前帧为关键帧。
可选的，当确定局部地图插入新的关键帧后，在当前已获取的局部地图中选择与新的关键帧满足共视关系的其他关键帧；
利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计；
当确定残差落入预设空间范围时，获取局部双目视觉地图位姿。
可选的，利用时间插值法，根据局部双目视觉地图位姿以及经过预处理后的局部三维激光点云图像，确定经过预处理后的局部三维激光点云图像中每一帧点云图像对应的位姿；
从经过预处理后的局部三维激光点云图像中提取预设帧数的点云图像，构建局部点云地图；
并将局部点云地图中首帧点云图像作为关键帧点云图像；
对新加入的一帧点云图像进行位姿优化；
并确定经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值；
当根据经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值，确定经过位姿优化后的点云图像为关键帧后，将经过位姿优化后的点云图像加入到局部点云地图中；
并以经过位姿优化后的点云图像作为当前关键帧，对下一帧新加入的图像执行重复执行以上操作步骤，直至所有点云图像执行以上操作步骤完成，获取局部融合地图。
可选的，从局部点云图像对应的角点集合中，查找与新加入的一帧点云图像中每一个角点分别对应的n个近邻角点，以及从局部点云图像对应的平点集合中，查找与新加入的一帧点云图像中每一个平点分别对应的j个近邻平点；
分别计算与第一角点对应的n个近邻角点的特征值，以及与第一平点对应的j个近邻平点的特征值；
当根据n个近邻角点分别对应的特征值，确定第一角点对应的临近点呈线状特征时，提取由n个近邻角点构成的直线中的第一预设数量点；
当根据j个近邻平点分别对应的特征值，确定第一平点对应的临近点呈面状特征时，提取由n个近邻平点构成的空间平面中的第二预设数量点；
根据第一预设数量点与其所在直线之间的对应关系、第二预设数量点与其所在空间平面之间的对应关系，以及新加入的一帧点云图像的位姿，构建代价函数；
利用代价函数，对新加入的一帧点云图像进行位姿优化，其中，第一角点为新加入的一帧点云图像中任一个角点，第一平点为新加入的一帧点云图像中任一平点，n和j均是正整数。
可选的，利用激光闭环检测方法获取局部融合地图中第一帧融合图像对应的临近帧图像；
获取第一帧融合图像和临近帧图像之间的相对位姿；
根据局部融合地图中每一帧融合图像、与每一帧融合图像对应的临近帧图像，以及二者之间的相对位置，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中，第一帧融合图像为局部融合地图中的任一帧融合图像。
上述本发明实施例揭示的方法可以应用于处理器601中，或者由处理器601实现。处理器601可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器601中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器601可以是通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(FieldProgrammable Gate Array， FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件单元组合执行完成。软件单元可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程 存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器602，处理器601读取存储器602中的信息，结合其硬件完成上述方法的步骤。
可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(ApplicationSpecific Integrated Circuits，ASIC)、数字信号处理器(Digital Signal Processing，DSP)、数字信号处理设备(DSPDevice，DSPD)、可编程逻辑设备(Programmable LogicDevice，PLD)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请功能的其它电子单元或其组合中。
对于软件实现，可通过执行本文功能的单元来实现本文的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。
本实施例提供的即时建图与定位系统可以是如图6中所示的即时建图与定位系统，可执行如图1中即时建图与定位方法的所有步骤，进而实现图1所示即时建图与定位方法的技术效果，具体请参照图1相关描述，为简洁描述，在此不作赘述。
本发明实施例还提供了一种存储介质（计算机可读存储介质）。这里的存储介质存储有一个或者多个程序。其中，存储介质可以包括易失性存储器，例如随机存取存储器；存储器也可以包括非易失性存储器，例如只读存储器、快闪存储器、硬盘或固态硬盘；存储器还可以包括上述种类的存储器的组合。
当存储介质中一个或者多个程序可被一个或者多个处理器执行，以实现上述在即时建图与定位系统侧执行的即时建图与定位方法。
处理器用于执行存储器中存储的即时建图与定位程序，以实现以下在即时建图与定位系统侧执行的即时建图与定位方法的步骤：
获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
分别对局部双目视觉图像和局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
将局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
根据局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中全局地图为根据全局三维激光点云图像构建的全局地图。
可选的，对局部双目视觉图像进行特征提取，获取局部双目视觉图像的特征点；
从局部双目视觉图像的特征点中筛选出符合预设条件的特定特征点；
计算特定特征点的特征描述子；
根据特征描述子，对双目视觉图像进行特征匹配，以便后续根据经过特征匹配后获取的局部双目视觉图像，构建局部地图。
可选的，从三维激光点云图像中提取地面点和非地面点；
并根据预设方法，区分地面点中的角点和平点；以及，区分非地面点中的角点和平点；
将地面点中的角点和非地面点中的角点共同构成角点集合，以及将地面点中的平点和非地面点中的平点共同构成平点集合。
可选的，根据经过预处理后的局部双目视觉图像，完成局部地图跟踪；
并在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
可选的，将经过处理后的局部双目视觉图像中首帧图像作为关键帧插入局部地图中；
以首帧图像作为参考帧，与新加入的当前帧图像进行特征点匹配；
根据首帧图像和当前帧图像之间相匹配的特征点，计算首帧图像和当前帧图像之间的位置之差和/或旋转角度之差；
当根据位置之差和/或旋转角度之差，确定当前帧为关键帧时，将当前帧插入到局部地图中；
并与下一个新加入帧图像重复执行以上操作步骤，直至局部双目视觉图像中的所有帧图像处理完毕，完成对局部地图跟踪。
可选的，当计算的是首帧图像和当前帧图像之间的位置之差，且位置之差大于或者等于第一预设阈值时，确定当前帧为关键帧；
或，当计算的是首帧图像和当前帧图像之间的旋转角度之差，且旋转角度之差大于或者等于第二预设阈值时，确定当前帧为关键帧；
或者，当计算的是首帧图像和当前帧图像之间的位置之差以及旋转角度之差，且位置之差和旋转角度之差均大于或者等于与之对应的预设阈值时，确定当前帧为关键帧。
可选的，当确定局部地图插入新的关键帧后，在当前已获取的局部地图中选择与新的关键帧满足共视关系的其他关键帧；
利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计；
当确定残差落入预设空间范围时，获取局部双目视觉地图位姿。
可选的，利用时间插值法，根据局部双目视觉地图位姿以及经过预处理后的局部三维激光点云图像，确定经过预处理后的局部三维激光点云图像中每一帧点云图像对应的位姿；
从经过预处理后的局部三维激光点云图像中提取预设帧数的点云图像，构建局部点云地图；
并将局部点云地图中首帧点云图像作为关键帧点云图像；
对新加入的一帧点云图像进行位姿优化；
并确定经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值；
当根据经过位姿优化后的点云图像与关键帧点云图像之间的位置差值和/或姿态差值，确定经过位姿优化后的点云图像为关键帧后，将经过位姿优化后的点云图像加入到局部点云地图中；
并以经过位姿优化后的点云图像作为当前关键帧，对下一帧新加入的图像执行重复执行以上操作步骤，直至所有点云图像执行以上操作步骤完成，获取局部融合地图。
可选的，从局部点云图像对应的角点集合中，查找与新加入的一帧点云图像中每一个角点分别对应的n个近邻角点，以及从局部点云图像对应的平点集合中，查找与新加入的一帧点云图像中每一个平点分别对应的j个近邻平点；
分别计算与第一角点对应的n个近邻角点的特征值，以及与第一平点对应的j个近邻平点的特征值；
当根据n个近邻角点分别对应的特征值，确定第一角点对应的临近点呈线状特征时，提取由n个近邻角点构成的直线中的第一预设数量点；
当根据j个近邻平点分别对应的特征值，确定第一平点对应的临近点呈面状特征时，提取由n个近邻平点构成的空间平面中的第二预设数量点；
根据第一预设数量点与其所在直线之间的对应关系、第二预设数量点与其所在空间平面之间的对应关系，以及新加入的一帧点云图像的位姿，构建代价函数；
利用代价函数，对新加入的一帧点云图像进行位姿优化，其中，第一角点为新加入的一帧点云图像中任一个角点，第一平点为新加入的一帧点云图像中任一平点，n和j均是正整数。
可选的，利用激光闭环检测方法获取局部融合地图中第一帧融合图像对应的临近帧图像；
获取第一帧融合图像和临近帧图像之间的相对位姿；
根据局部融合地图中每一帧融合图像、与每一帧融合图像对应的临近帧图像，以及二者之间的相对位置，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中，第一帧融合图像为局部融合地图中的任一帧融合图像。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器（RAM）、内存、只读存储器（ROM）、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (13)

1.一种即时建图与定位方法，其特征在于，所述方法包括：
获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
分别对所述局部双目视觉图像和所述局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
将所述局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
根据所述局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中所述全局地图为根据全局三维激光点云图像构建的全局地图。
2.根据权利要求1所述的方法，其特征在于，所述对所述局部双目视觉图像进行预处理，具体包括：
对所述局部双目视觉图像进行特征提取，获取所述局部双目视觉图像的特征点；
从所述局部双目视觉图像的特征点中筛选出符合预设条件的特定特征点；
计算所述特定特征点的特征描述子；
根据所述特征描述子，对所述双目视觉图像进行特征匹配，以便后续根据经过所述特征匹配后获取的局部双目视觉图像，构建所述局部地图。
3.根据权利要求1所述的方法，其特征在于，对所述局部三维激光点云图像进行预处理，具体包括：
从所述三维激光点云图像中提取地面点和非地面点；
并根据预设方法，区分所述地面点中的角点和平点；以及，区分所述非地面点中的角点和平点；
将所述地面点中的角点和所述非地面点中的角点共同构成角点集合，以及将所述地面点中的平点和所述非地面点中的平点共同构成平点集合。
4.根据权利要求2所述的方法，其特征在于，所述根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿，具体包括：
根据经过预处理后的局部双目视觉图像，完成局部地图跟踪；
并在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿。
5.根据权利要求4所述的方法，其特征在于，根据经过预处理后的局部双目视觉图像，完成具体地图跟踪，具体包括：
将所述经过处理后的局部双目视觉图像中首帧图像作为关键帧插入局部地图中；
以所述首帧图像作为参考帧，与新加入的当前帧图像进行特征点匹配；
根据所述首帧图像和所述当前帧图像之间相匹配的特征点，计算所述首帧图像和所述当前帧图像之间的位置之差和/或旋转角度之差；
当根据所述位置之差和/或所述旋转角度之差，确定所述当前帧为关键帧时，将所述当前帧插入到所述局部地图中；
并与下一个新加入帧图像重复执行以上操作步骤，直至所述局部双目视觉图像中的所有帧图像处理完毕，完成对所述局部地图跟踪。
6.根据权利要求5所述的方法，其特征在于，根据所述位置之差和/或所述旋转角度之差，确定所述当前帧为关键帧，具体包括：
当计算的是所述首帧图像和所述当前帧图像之间的位置之差，且所述位置之差大于或者等于第一预设阈值时，确定所述当前帧为关键帧；
或，当计算的是所述首帧图像和所述当前帧图像之间的旋转角度之差，且所述旋转角度之差大于或者等于第二预设阈值时，确定所述当前帧为关键帧；
或者，当计算的是所述首帧图像和所述当前帧图像之间的位置之差以及旋转角度之差，且所述位置之差和所述旋转角度之差均大于或者等于与之对应的预设阈值时，确定所述当前帧为关键帧。
7.根据权利要求5所述的方法，其特征在于，所述在地图跟踪过程中，对当前已获取的局部地图进行优化，获取局部双目视觉地图位姿，具体包括：
当确定所述局部地图插入新的关键帧后，在所述当前已获取的局部地图中选择与所述新的关键帧满足共视关系的其他关键帧；
利用局部光束法平差对所有满足共视关系的关键帧之间的残差进行最小二乘估计；
当确定所述残差落入预设空间范围时，获取所述局部双目视觉地图位姿。
8.根据权利要求3所述的方法，其特征在于，所述将所述局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图，具体包括：
利用时间插值法，根据所述局部双目视觉地图位姿以及所述经过预处理后的局部三维激光点云图像，确定所述经过预处理后的局部三维激光点云图像中每一帧点云图像对应的位姿；
从所述经过预处理后的局部三维激光点云图像中提取预设帧数的点云图像，构建局部点云地图；
并将所述局部点云地图中首帧点云图像作为关键帧点云图像；
对新加入的一帧点云图像进行位姿优化；
并确定经过位姿优化后的点云图像与所述关键帧点云图像之间的位置差值和/或姿态差值；
当根据所述经过位姿优化后的点云图像与所述关键帧点云图像之间的位置差值和/或姿态差值，确定所述经过位姿优化后的点云图像为关键帧后，将所述经过位姿优化后的点云图像加入到所述局部点云地图中；
并以所述经过位姿优化后的点云图像作为当前关键帧，对下一帧新加入的图像执行重复执行以上操作步骤，直至所有点云图像执行以上操作步骤完成，获取局部融合地图。
9.根据权利要求8所述的方法，其特征在于，所述对新加入的一帧点云图像进行位姿优化，具体包括：
从所述局部点云图像对应的角点集合中，查找与所述新加入的一帧点云图像中每一个角点分别对应的n个近邻角点，以及从所述局部点云图像对应的平点集合中，查找与所述新加入的一帧点云图像中每一个平点分别对应的j个近邻平点；
分别计算与第一角点对应的n个近邻角点的特征值，以及与第一平点对应的j个近邻平点的特征值；
当根据所述n个近邻角点分别对应的特征值，确定所述第一角点对应的临近点呈线状特征时，提取由所述n个近邻角点构成的直线中的第一预设数量点；
当根据所述j个近邻平点分别对应的特征值，确定所述第一平点对应的临近点呈面状特征时，提取所述由所述n个近邻平点构成的空间平面中的第二预设数量点；
根据所述第一预设数量点与其所在直线之间的对应关系、所述第二预设数量点与其所在空间平面之间的对应关系，以及所述新加入的一帧点云图像的位姿，构建代价函数；
利用所述代价函数，对所述新加入的一帧点云图像进行位姿优化，其中，所述第一角点为所述新加入的一帧点云图像中任一个角点，所述第一平点为所述新加入的一帧点云图像中任一平点，n和j均是正整数。
10.根据权利要求1-9任一项所述的方法，其特征在于，所述根据所述局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，具体包括：
利用激光闭环检测方法获取所述局部融合地图中第一帧融合图像对应的临近帧图像；
获取所述第一帧融合图像和所述临近帧图像之间的相对位姿；
根据所述局部融合地图中每一帧融合图像、与每一帧融合图像对应的临近帧图像，以及二者之间的相对位置，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中，所述第一帧融合图像为所述局部融合地图中的任一帧融合图像。
11.一种即时建图与定位装置，其特征在于，所述装置包括：
获取单元，用于获取当前所能采集的局部双目视觉图像以及局部三维激光点云图像；
处理单元，用于分别对所述局部双目视觉图像和所述局部三维激光点云图像进行预处理；
根据经过预处理后的局部双目视觉图像，获取局部双目视觉地图位姿；
融合单元，用于将所述局部双目视觉地图位姿与经过预处理后的局部三维激光点云图像融合，获取局部融合地图；
地图优化单元，用于根据所述局部融合地图，对全局地图进行全局一致性优化，获取稠密点云地图，并实时输出当前时刻的位置信息和姿态信息，完成即时建图与定位，其中所述全局地图为根据全局三维激光点云图像构建的全局地图。
12.一种即时建图与定位系统，其特征在于，所述系统包括：至少一个处理器和存储器；
所述处理器用于执行所述存储器中存储的即时建图与定位程序，以实现权利要求1~10中任一项所述的即时建图与定位方法。
13.一种计算机存储介质，其特征在于，所述计算机存储介质存储有一个或者多个程序，所述一个或者多个程序可被如权利要求12所述的即时建图与定位系统执行，以实现权利要求1~10中任一项所述的即时建图与定位方法。
CN202010416829.5A 2020-05-18 2020-05-18 即时建图与定位方法、装置、系统及存储介质 Active CN111337947B (zh)

Priority Applications (4)

Application Number Priority Date Filing Date Title
CN202010416829.5A CN111337947B (zh) 2020-05-18 2020-05-18 即时建图与定位方法、装置、系统及存储介质
PCT/CN2021/087531 WO2021233029A1 (en) 2020-05-18 2021-04-15 Simultaneous localization and mapping method, device, system and storage medium
US17/624,317 US20230260151A1 (en) 2020-05-18 2021-04-15 Simultaneous Localization and Mapping Method, Device, System and Storage Medium
EP21728155.9A EP3977346A1 (en) 2020-05-18 2021-04-15 Simultaneous localization and mapping method, device, system and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010416829.5A CN111337947B (zh) 2020-05-18 2020-05-18 即时建图与定位方法、装置、系统及存储介质

Publications (2)

Publication Number Publication Date
CN111337947A true CN111337947A (zh) 2020-06-26
CN111337947B CN111337947B (zh) 2020-09-22

Family

ID=71184911

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010416829.5A Active CN111337947B (zh) 2020-05-18 2020-05-18 即时建图与定位方法、装置、系统及存储介质

Country Status (4)

Country Link
US (1) US20230260151A1 (zh)
EP (1) EP3977346A1 (zh)
CN (1) CN111337947B (zh)
WO (1) WO2021233029A1 (zh)

Cited By (24)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112016513A (zh) * 2020-09-08 2020-12-01 北京达佳互联信息技术有限公司 视频语义分割方法、模型训练方法、相关装置及电子设备
CN112258646A (zh) * 2020-10-26 2021-01-22 上海汽车集团股份有限公司 一种三维线地标构建方法及装置
CN112258568A (zh) * 2020-10-12 2021-01-22 武汉中海庭数据技术有限公司 一种高精度地图要素的提取方法及装置
CN112269187A (zh) * 2020-09-28 2021-01-26 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN112506200A (zh) * 2020-12-14 2021-03-16 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN112596064A (zh) * 2020-11-30 2021-04-02 中科院软件研究所南京软件技术研究院 激光与视觉融合的一体化室内机器人全局定位方法
CN112668585A (zh) * 2020-11-26 2021-04-16 厦门大学 一种动态环境下的物体辨识与定位方法
CN112698306A (zh) * 2020-12-17 2021-04-23 上海交通大学宁波人工智能研究院 一种多激光雷达结合相机解决地图构建盲区的系统和方法
CN112747749A (zh) * 2020-12-23 2021-05-04 浙江同筑科技有限公司 一种基于双目视觉和激光融合定位导航系统
CN113110455A (zh) * 2021-04-16 2021-07-13 哈尔滨工业大学 一种未知初始状态的多机器人协同探索方法、装置及系统
CN113379915A (zh) * 2021-07-05 2021-09-10 广东工业大学 一种基于点云融合的行车场景构建方法
CN113432600A (zh) * 2021-06-09 2021-09-24 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及系统
CN113447014A (zh) * 2021-08-30 2021-09-28 深圳市大道智创科技有限公司 室内移动机器人、建图方法、定位方法以及建图定位装置
CN113503883A (zh) * 2021-06-22 2021-10-15 北京三快在线科技有限公司 采集用于构建地图的数据的方法、存储介质及电子设备
CN113763468A (zh) * 2021-01-21 2021-12-07 北京京东乾石科技有限公司 一种定位方法、装置、系统及存储介质
CN114024944A (zh) * 2021-11-02 2022-02-08 广州虎牙科技有限公司 媒体内容植入方法、装置、电子设备及存储介质
CN114742884A (zh) * 2022-06-09 2022-07-12 杭州迦智科技有限公司 一种基于纹理的建图、里程计算、定位方法及系统
CN115267796A (zh) * 2022-08-17 2022-11-01 深圳市普渡科技有限公司 定位方法、装置、机器人和存储介质
CN115601434A (zh) * 2022-12-12 2023-01-13 安徽蔚来智驾科技有限公司（Cn） 回环检测方法、计算机设备、计算机可读存储介质及车辆
WO2023093515A1 (zh) * 2021-11-29 2023-06-01 珠海一微半导体股份有限公司 一种基于扇面深度相机的定位系统及定位方法
CN116883502A (zh) * 2023-09-05 2023-10-13 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
CN117237553A (zh) * 2023-09-14 2023-12-15 广东省核工业地质局测绘院 一种基于点云图像融合的三维地图测绘系统
CN117649536A (zh) * 2024-01-29 2024-03-05 华东交通大学 一种点线和线结构特征融合的视觉同步定位与建图方法

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114418952A (zh) * 2021-12-21 2022-04-29 未来机器人（深圳）有限公司 货物清点方法、装置、计算机设备、存储介质
CN114280583B (zh) * 2022-03-02 2022-06-17 武汉理工大学 无gps信号下激光雷达定位精度验证方法及系统
CN114332233B (zh) * 2022-03-17 2022-05-31 北京理工大学 一种激光slam回环检测方法和系统
CN114543787B (zh) * 2022-04-21 2022-09-13 南京理工大学 基于条纹投影轮廓术的毫米级室内建图定位方法
CN114998408B (zh) * 2022-04-26 2023-06-06 宁波益铸智能科技有限公司 基于激光测量的冲床线ccd视觉检测系统
CN115661395B (zh) * 2022-12-27 2023-04-11 安徽蔚来智驾科技有限公司 车位建图方法、车辆及存储介质
CN116071283B (zh) * 2023-04-07 2023-06-16 湖南腾琨信息科技有限公司 基于计算机视觉的三维点云图像融合方法
CN116660916B (zh) * 2023-05-26 2024-02-02 广东省农业科学院设施农业研究所 一种用于果园移动机器人的定位方法、建图方法及电子设备
CN117132728B (zh) * 2023-10-26 2024-02-23 毫末智行科技有限公司 构建地图的方法、装置、电子设备及存储介质
CN117456108B (zh) * 2023-12-22 2024-02-23 四川省安全科学技术研究院 一种线激光传感器与高清摄像头的三维数据采集方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104700402A (zh) * 2015-02-06 2015-06-10 北京大学 基于场景三维点云的视觉定位方法及装置
CN105469405A (zh) * 2015-11-26 2016-04-06 清华大学 基于视觉测程的同时定位与地图构建方法
US9519979B1 (en) * 2015-01-23 2016-12-13 The United States Of America As Represented By The Secretary Of The Navy Ladar range data video color rendering
CN107796397A (zh) * 2017-09-14 2018-03-13 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
CN110132278A (zh) * 2019-05-14 2019-08-16 驭势科技（北京）有限公司 一种即时定位与建图的方法及装置
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110322500A (zh) * 2019-06-28 2019-10-11 Oppo广东移动通信有限公司 即时定位与地图构建的优化方法及装置、介质和电子设备
CN110389348A (zh) * 2019-07-30 2019-10-29 四川大学 基于激光雷达与双目相机的定位与导航方法及装置
CN110568447A (zh) * 2019-07-29 2019-12-13 广东星舆科技有限公司 视觉定位的方法、装置及计算机可读介质
CN110853075A (zh) * 2019-11-05 2020-02-28 北京理工大学 一种基于稠密点云与合成视图的视觉跟踪定位方法
CN110910498A (zh) * 2019-11-21 2020-03-24 大连理工大学 一种利用激光雷达和双目相机构建栅格地图的方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN111105495A (zh) * 2019-11-26 2020-05-05 四川阿泰因机器人智能装备有限公司 一种融合视觉语义信息的激光雷达建图方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110361027A (zh) * 2019-06-25 2019-10-22 马鞍山天邦开物智能商务管理有限公司 基于单线激光雷达与双目相机数据融合的机器人路径规划方法

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9519979B1 (en) * 2015-01-23 2016-12-13 The United States Of America As Represented By The Secretary Of The Navy Ladar range data video color rendering
CN104700402A (zh) * 2015-02-06 2015-06-10 北京大学 基于场景三维点云的视觉定位方法及装置
CN105469405A (zh) * 2015-11-26 2016-04-06 清华大学 基于视觉测程的同时定位与地图构建方法
CN107796397A (zh) * 2017-09-14 2018-03-13 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
CN110261870A (zh) * 2019-04-15 2019-09-20 浙江工业大学 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN110132278A (zh) * 2019-05-14 2019-08-16 驭势科技（北京）有限公司 一种即时定位与建图的方法及装置
CN110322500A (zh) * 2019-06-28 2019-10-11 Oppo广东移动通信有限公司 即时定位与地图构建的优化方法及装置、介质和电子设备
CN110568447A (zh) * 2019-07-29 2019-12-13 广东星舆科技有限公司 视觉定位的方法、装置及计算机可读介质
CN110389348A (zh) * 2019-07-30 2019-10-29 四川大学 基于激光雷达与双目相机的定位与导航方法及装置
CN110853075A (zh) * 2019-11-05 2020-02-28 北京理工大学 一种基于稠密点云与合成视图的视觉跟踪定位方法
CN110910498A (zh) * 2019-11-21 2020-03-24 大连理工大学 一种利用激光雷达和双目相机构建栅格地图的方法
CN111105495A (zh) * 2019-11-26 2020-05-05 四川阿泰因机器人智能装备有限公司 一种融合视觉语义信息的激光雷达建图方法及系统
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法

Cited By (39)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111983639B (zh) * 2020-08-25 2023-06-02 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN111983639A (zh) * 2020-08-25 2020-11-24 浙江光珀智能科技有限公司 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN112016513A (zh) * 2020-09-08 2020-12-01 北京达佳互联信息技术有限公司 视频语义分割方法、模型训练方法、相关装置及电子设备
CN112016513B (zh) * 2020-09-08 2024-01-30 北京达佳互联信息技术有限公司 视频语义分割方法、模型训练方法、相关装置及电子设备
CN112269187A (zh) * 2020-09-28 2021-01-26 广州视源电子科技股份有限公司 机器人状态检测方法、装置及设备
CN112258568B (zh) * 2020-10-12 2022-07-01 武汉中海庭数据技术有限公司 一种高精度地图要素的提取方法及装置
CN112258568A (zh) * 2020-10-12 2021-01-22 武汉中海庭数据技术有限公司 一种高精度地图要素的提取方法及装置
CN112258646A (zh) * 2020-10-26 2021-01-22 上海汽车集团股份有限公司 一种三维线地标构建方法及装置
CN112258646B (zh) * 2020-10-26 2024-03-12 上海汽车集团股份有限公司 一种三维线地标构建方法及装置
CN112668585B (zh) * 2020-11-26 2024-04-09 厦门大学 一种动态环境下的物体辨识与定位方法
CN112668585A (zh) * 2020-11-26 2021-04-16 厦门大学 一种动态环境下的物体辨识与定位方法
CN112596064B (zh) * 2020-11-30 2024-03-08 中科院软件研究所南京软件技术研究院 激光与视觉融合的一体化室内机器人全局定位方法
CN112596064A (zh) * 2020-11-30 2021-04-02 中科院软件研究所南京软件技术研究院 激光与视觉融合的一体化室内机器人全局定位方法
CN112506200B (zh) * 2020-12-14 2023-12-08 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN112506200A (zh) * 2020-12-14 2021-03-16 广州视源电子科技股份有限公司 机器人定位方法、装置、机器人及存储介质
CN112698306A (zh) * 2020-12-17 2021-04-23 上海交通大学宁波人工智能研究院 一种多激光雷达结合相机解决地图构建盲区的系统和方法
CN112747749A (zh) * 2020-12-23 2021-05-04 浙江同筑科技有限公司 一种基于双目视觉和激光融合定位导航系统
CN112747749B (zh) * 2020-12-23 2022-12-06 浙江同筑科技有限公司 一种基于双目视觉和激光融合定位导航系统
CN113763468B (zh) * 2021-01-21 2023-12-05 北京京东乾石科技有限公司 一种定位方法、装置、系统及存储介质
CN113763468A (zh) * 2021-01-21 2021-12-07 北京京东乾石科技有限公司 一种定位方法、装置、系统及存储介质
CN113110455B (zh) * 2021-04-16 2022-09-23 哈尔滨工业大学 一种未知初始状态的多机器人协同探索方法、装置及系统
CN113110455A (zh) * 2021-04-16 2021-07-13 哈尔滨工业大学 一种未知初始状态的多机器人协同探索方法、装置及系统
CN113432600B (zh) * 2021-06-09 2022-08-16 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及系统
CN113432600A (zh) * 2021-06-09 2021-09-24 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及系统
CN113503883A (zh) * 2021-06-22 2021-10-15 北京三快在线科技有限公司 采集用于构建地图的数据的方法、存储介质及电子设备
CN113379915A (zh) * 2021-07-05 2021-09-10 广东工业大学 一种基于点云融合的行车场景构建方法
CN113447014A (zh) * 2021-08-30 2021-09-28 深圳市大道智创科技有限公司 室内移动机器人、建图方法、定位方法以及建图定位装置
CN114024944B (zh) * 2021-11-02 2024-03-29 广州虎牙科技有限公司 媒体内容植入方法、装置、电子设备及存储介质
CN114024944A (zh) * 2021-11-02 2022-02-08 广州虎牙科技有限公司 媒体内容植入方法、装置、电子设备及存储介质
WO2023093515A1 (zh) * 2021-11-29 2023-06-01 珠海一微半导体股份有限公司 一种基于扇面深度相机的定位系统及定位方法
CN114742884A (zh) * 2022-06-09 2022-07-12 杭州迦智科技有限公司 一种基于纹理的建图、里程计算、定位方法及系统
CN114742884B (zh) * 2022-06-09 2022-11-22 杭州迦智科技有限公司 一种基于纹理的建图、里程计算、定位方法及系统
CN115267796B (zh) * 2022-08-17 2024-04-09 深圳市普渡科技有限公司 定位方法、装置、机器人和存储介质
CN115267796A (zh) * 2022-08-17 2022-11-01 深圳市普渡科技有限公司 定位方法、装置、机器人和存储介质
CN115601434A (zh) * 2022-12-12 2023-01-13 安徽蔚来智驾科技有限公司（Cn） 回环检测方法、计算机设备、计算机可读存储介质及车辆
CN116883502A (zh) * 2023-09-05 2023-10-13 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
CN117237553A (zh) * 2023-09-14 2023-12-15 广东省核工业地质局测绘院 一种基于点云图像融合的三维地图测绘系统
CN117649536A (zh) * 2024-01-29 2024-03-05 华东交通大学 一种点线和线结构特征融合的视觉同步定位与建图方法
CN117649536B (zh) * 2024-01-29 2024-04-16 华东交通大学 一种点线和线结构特征融合的视觉同步定位与建图方法

Also Published As

Publication number Publication date
US20230260151A1 (en) 2023-08-17
CN111337947B (zh) 2020-09-22
WO2021233029A1 (en) 2021-11-25
EP3977346A1 (en) 2022-04-06

Similar Documents

Publication Publication Date Title
CN111337947B (zh) 即时建图与定位方法、装置、系统及存储介质
CN112785702B (zh) 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN111983639B (zh) 一种基于Multi-Camera/Lidar/IMU的多传感器SLAM方法
CN108717710B (zh) 室内环境下的定位方法、装置及系统
US10953545B2 (en) System and method for autonomous navigation using visual sparse map
CN108369743B (zh) 使用多方向相机地图构建空间
CN110176032B (zh) 一种三维重建方法及装置
CN107167826B (zh) 一种自动驾驶中基于可变网格的图像特征检测的车辆纵向定位系统及方法
Qiu et al. Model-based global localization for aerial robots using edge alignment
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN113865580A (zh) 构建地图的方法、装置、电子设备及计算机可读存储介质
CN110597265A (zh) 一种扫地机器人回充方法和装置
CN112556685A (zh) 导航路线的显示方法、装置和存储介质及电子设备
CN112068152A (zh) 使用3d扫描仪同时进行2d定位和2d地图创建的方法和系统
Manivannan et al. Vision based intelligent vehicle steering control using single camera for automated highway system
CN114662587A (zh) 一种基于激光雷达的三维目标感知方法、装置及系统
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
Qayyum et al. Imu aided rgb-d slam
Shacklock et al. Visual guidance for autonomous vehicles: capability and challenges
Hoang et al. Combining edge and one-point ransac algorithm to estimate visual odometry
Sahdev et al. Indoor localization in dynamic human environments using visual odometry and global pose refinement
Goronzy et al. QRPos: Indoor positioning system for self-balancing robots based on QR codes
US11763492B1 (en) Apparatus and methods to calibrate a stereo camera pair
CN113379850B (zh) 移动机器人控制方法、装置、移动机器人及存储介质
CN112767482B (zh) 一种多传感器融合的室内外定位方法及系统

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