CN112101160B - 一种面向自动驾驶场景的双目语义slam方法 - Google Patents
一种面向自动驾驶场景的双目语义slam方法 Download PDFInfo
- Publication number
- CN112101160B CN112101160B CN202010922292.XA CN202010922292A CN112101160B CN 112101160 B CN112101160 B CN 112101160B CN 202010922292 A CN202010922292 A CN 202010922292A CN 112101160 B CN112101160 B CN 112101160B
- Authority
- CN
- China
- Prior art keywords
- frame
- pose
- binocular
- objects
- tracking
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000033001 locomotion Effects 0.000 claims abstract description 24
- 230000003068 static effect Effects 0.000 claims abstract description 20
- 238000005457 optimization Methods 0.000 claims abstract description 12
- 238000013528 artificial neural network Methods 0.000 claims abstract description 9
- 238000001514 detection method Methods 0.000 claims description 23
- 238000013519 translation Methods 0.000 claims description 14
- 230000011218 segmentation Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 5
- 238000012795 verification Methods 0.000 claims description 5
- 230000009977 dual effect Effects 0.000 claims description 4
- 238000000605 extraction Methods 0.000 claims description 4
- 230000009466 transformation Effects 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 claims description 2
- 238000003062 neural network model Methods 0.000 claims description 2
- 230000000007 visual effect Effects 0.000 description 9
- 230000000694 effects Effects 0.000 description 7
- 238000012360 testing method Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- 238000007476 Maximum Likelihood Methods 0.000 description 2
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 2
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241000764238 Isis Species 0.000 description 1
- 125000000205 L-threonino group Chemical group [H]OC(=O)[C@@]([H])(N([H])[*])[C@](C([H])([H])[H])([H])O[H] 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 238000013527 convolutional neural network Methods 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000004880 explosion Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
- G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/24—Classification techniques
- G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
- G06F18/2415—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches based on parametric or probabilistic models, e.g. based on likelihood ratio or false acceptance rate versus a false rejection rate
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/04—Architecture, e.g. interconnection topology
- G06N3/045—Combinations of networks
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/02—Neural networks
- G06N3/08—Learning methods
- G06N3/082—Learning methods modifying the architecture, e.g. adding, deleting or silencing nodes or connections
-
- 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/20—Image preprocessing
- G06V10/25—Determination of region of interest [ROI] or a volume of interest [VOI]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/40—Scenes; Scene-specific elements in video content
- G06V20/41—Higher-level, semantic clustering, classification or understanding of video scenes, e.g. detection, labelling or Markovian modelling of sport events or news items
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Multimedia (AREA)
- Life Sciences & Earth Sciences (AREA)
- Artificial Intelligence (AREA)
- Software Systems (AREA)
- Computational Linguistics (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- General Health & Medical Sciences (AREA)
- Health & Medical Sciences (AREA)
- Biomedical Technology (AREA)
- Biophysics (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- Mathematical Physics (AREA)
- Evolutionary Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Computational Biology (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Probability & Statistics with Applications (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种面向自动驾驶场景的双目语义SLAM方法，该方法能够在自动驾驶场景中，使车辆在准确跟踪自身在环境中位姿的同时，检测并跟踪场景中的其他车辆、行人等物体。该方法使用二次曲面作为三维物体的表达，与之前的方法仅可以将二次曲面用作对静态物体建图或者做路标相比，本发明无论是对于场景中的静态物体，还是运动中的物体，都可以用二次曲面表达不同时刻物体的状态。针对语义物体观测不足的情况，本发明充分利用神经网络和先验信息进行约束，构建了对物体观测的误差项，实现了对物体状态参数的非线性优化求解。
Description
技术领域
本发明涉及同时定位和地图构建(SLAM)领域，具体涉及一种面向自动驾驶场景的双目语义SLAM方法。
背景技术
同时定位和地图构建(Simultaneous Localization and Mapping，SLAM)是近年来热门的研究课题之一，它是指使用搭载特定传感器的设备，在未知环境中对传感器接收到的信息进行处理，从而估计自身在环境中的位置和朝向，以及建立特定格式的环境地图的算法。随着移动机器人、无人机等设备越来越多地出现在工业和家用环境，增强现实领域的蓬勃发展，以及更多如自动驾驶等新兴技术的方兴未艾，SLAM技术作为这些领域的关键算法之一，越来越受到工业界和学术界的广泛关注。SLAM常用的传感器有摄像头、深度相机、激光雷达、惯性测量单元(IMU)等，根据目标需求不同可以采用单个或者多个同种或不同种传感器的组合。其中摄像头作为最常见，同时又成本低廉的传感器，使得视觉SLAM 得到了广泛而深入的研究。
而近年来人工智能技术的再次兴起，使计算机视觉中的很多关键问题都取得了革命性的进展，例如计算机可以更轻易地识别一幅图像中哪里有感兴趣的物体 (目标检测)，该物体是什么类别(物体分类)，该物体的精确边界是什么(实例分割)，它们都属于场景理解的范畴。这些任务的突破性进展，使得实现更高级、更智能的任务成为了可能。例如语义SLAM就是将它们检测到的语义信息与传统SLAM相结合而兴起的新的研究方向。传统SLAM所检测到的环境信息都停留在低级别的点、线、面，并不知道环境中有什么物体、在什么地方，而有了语义信息，就可以帮助实现更多功能，例如利用语义来帮助定位、建立带有语义信息的地图、估计相机自身位姿的同时估计物体的位姿等。
自动驾驶任务中对环境感知和自身定位有着强烈的需求，只有能准确地感知环境并精确地跟踪自身位置和姿态，才可能做出更安全、更合理的决策。语义 SLAM的出现则正好可以契合这一需求，可以使用SLAM追踪车辆位姿，同时对检测到的语义物体设计可行的方法进行跟踪。
近年来，有许多研究者提出了很多不同的语义SLAM方法。
研究人员提出的DynaSLAM将实例分割方法Mask R-CNN与ORB-SLAM2 结合，提高了SLAM对动态场景的适应性。该方法将实例分割结果中的物体赋予动态先验，然后跟踪时将属于动态物体的特征点排除在外，同时在建图时忽略动态物体的数据，并利用背景修复的方法，得到静态场景的地图。该方法仅使用实例分割为SLAM提供了动态先验，无法对物体本身进行跟踪。
研究人员提出了用二次曲面来表达物体的QuadricSLAM。该方法使用受约束的二次曲面——椭球面来表达物体的位姿和尺寸，通过目标检测得到的二维包围框作为约束，不断优化椭球面参数，得到物体的状态，并且在跟踪时将物体作为路标帮助定位。该方法的缺点是，物体的数据关联是离线做好的，缺乏在线数据关联的手段，因此无法在线运行；所表达的物体只能是静态物体，无法跟踪动态物体；对物体没有加入先验的约束，如果对物体观测不足，那么椭球面参数无法很好地收敛。
研究人员针对自动驾驶场景提出了使用双目相机同时进行自我跟踪和语义物体跟踪的方法。他们使用三维包围框作为场景中车辆的表达，利用二维物体检测和车辆视角回归的方式来回归包围框参数，使用得到的三维包围框生成掩膜来区分背景和车辆的特征点，属于背景的特征点用于相机跟踪，属于车辆的特征点进行特征点匹配来关联不同帧的车辆，跟踪车辆时使用二维包围框、特征点、车辆尺寸先验和车辆运动模型构建约束进行优化。该方法的缺点是车辆的朝向仅能取离散值，参数表达不够精确；用三维包围框生成的掩膜不够精细，更容易造成物体误匹配的情况；将所有物体都视为动态物体，相机跟踪时无法利用静态物体的信息。
发明内容
本发明的目的在于解决现有技术中存在的问题，本发明提出了一种面向自动驾驶场景的双目语义SLAM方法，该方法能够在自动驾驶场景中，使车辆在准确跟踪自身在环境中位姿的同时，检测并跟踪场景中的其他车辆、行人等物体。
为了实现上述目的，本发明采用如下技术方案：
一种面向自动驾驶场景的双目语义SLAM方法，包括如下步骤：
1)通过神经网络模型检测左目图像中的物体，得到物体的二维包围框、掩膜及朝向；
2)采用ORB特征点法提取双目图像的特征点，根据神经网络得到的掩膜赋予特征点先验信息，利用属于背景的特征点以及物体跟踪后属于静态物体的特征点对双目相机进行跟踪，得到相机的位姿，并生成关键帧，然后将关键帧信息传输到后端进行建图和回环检测；
3)使用二次曲面表达物体，采用特征点匹配的方式关联前后帧物体，利用二维包围框和二次曲面投影之间的约束以及先验信息提供的约束，不断跟踪和优化得到物体不同时刻的相对位姿，而后利用相机跟踪结果得到物体的绝对位姿，最后利用物体跟踪的结果为下一帧的特征点提供先验信息，提高相机跟踪的精度和鲁棒性。
进一步的，所述的步骤1)包括如下步骤：
1.1)使用目标检测网络，对左目图像中的物体进行检测，得到物体的二维包围框；
1.2)对检测到的物体使用朝向预测网络，预测每个物体的朝向先验；
1.3)使用实例分割网络，将物体的二维包围框作为实例分割网络中区域提议网络部分的输出，得到每个物体的掩膜先验。
进一步的，所述的步骤2)包括如下步骤：
2.1)双目初始化；
2.2)初始化成功后，对双目相机进行跟踪，采用恒速模型预测当前帧位姿，并对当前帧位姿进行3D-2D的位姿优化；而后再与局部地图中的地图点进行匹配，并做3D-2D的位姿优化，最终得到优化后的当前帧位姿；
2.3)若跟踪过程中有效匹配的特征点低于阈值，则认为跟踪失败，进入重定位流程；若跟踪成功，则更新物体的绝对位姿并生成关键帧，由生成关键帧进行建图，并不断进行回环检测。
进一步的，刚完成双目初始化时，恒速模型还未生成，此时需要先将当前帧与参考关键帧匹配，再与局部地图匹配，最终优化得到当前帧位姿。
进一步的，所述的步骤2.1)具体为：
2.1.1)接收双目图像流，分别对左目图像和右目图像进行灰度图处理；
2.1.2)对当前帧的左目图像和右目图像进行ORB特征点提取，然后根据神经网络生成的掩膜先验，将左目图像中的特征点标记为属于某个物体或属于背景；
2.1.3)进行双目特征点匹配，若双目特征点匹配的数量大于阈值，则利用三角化生成初始的地图点，双目初始化完成；否则，重复步骤2.1.2)至步骤2.1.3)，用后续帧继续进行初始化，直至双目初始化完成。
进一步的，步骤2.3)所述的由生成关键帧进行建图，并不断进行回环检测，具体为：
2.3.1)建图过程中，首先将新的关键帧插入到共视图中，更新有共视关系的关键帧的边及生成树，计算新的关键帧的词袋向量；
2.3.2)剔除冗余的地图点，利用三角化生成新的地图，并对窗口范围内的关键帧和地图点进行局部集束调整，优化关键帧的位姿和地图点的位置，随后进行关键帧剔除，若某一关键帧90％以上地图点能够被其他3个以上关键帧观测到，则剔除该关键帧；
2.3.3)进行回环检测，计算当前关键帧及共视图中附近的关键帧的相似度得分，将分数最低的一个关键帧与识别数据库中的闭环候选关键帧进行比较，若相似性达到一定阈值，则对当前关键帧及其相邻两帧、闭环候选关键帧及其相邻两帧进行交叉验证；根据交叉验证结果，首先使用相似变换群对当前关键帧的位姿进行矫正，再纠正相邻关键帧以及相邻关键帧能够观察到的地图点，之后对本质图进行优化，最后再使用全局集束调整对所有关键帧的位姿和地图点进行优化。
进一步的，所述步骤2.3.2)中剔除冗余的地图点时，对已标记为动态的地图点进行剔除，同时生成新地图点时也会跳过被标记为动态的特征点。
进一步的，所述的步骤3)包括如下步骤：
3.1)使用二次曲面表达物体，进行物体初始化；
3.2)对物体进行特征点关联：
对于上一帧中的每个物体，在当前帧中相同位置区域内从最近的物体开始进行特征点匹配；
若特征点的匹配数量多于预设值，则物体匹配成功，匹配的特征点数量最多的候选物体认为与上一帧中相匹配的物体是同一个物体，更新该物体的运动状态为该物体在上一帧中的运动状态，更新物体ID和物体位姿，并将当前帧中的物体与上一帧中的物体同时优化，更新物体的绝对位姿；
若特征点匹配数量少于预设值，则认为匹配不成功，需寻找参考帧进行匹配，将匹配成功的参考帧中的物体认为是同一个物体，沿用参考帧中物体的运动状态，更新物体ID和物体位姿，并将当前帧中的物体与参考帧中的物体同时优化，更新物体的绝对位姿；若与参考帧中物体匹配的特征点仍然少于预设值，即匹配不成功，则当前帧中的物体为新的物体，对新的物体进行优化，更新物体的绝对位姿；
3.3)判断物体的运动状态，若物体的运动状态发生改变，则将物体关联的特征点、以及相应的地图点标记为动态或者静止，标记为动态的特征点不会被生成新的地图点，标记为动态的地图点不会参与相机的跟踪，也不会参与局部集束调整，直接会被视作外点，在后续的相机跟踪中被剔除。
进一步的，物体的运动状态分为动态或者静止，判断物体运动状态的方法为：
首先将所有物体的状态初始化为动态，然后对物体在世界坐标系下的平移变化量进行判断，采用滑动窗口形式，记录物体最近6帧范围内的平移，如果物体在3帧范围内平移超过阈值，则更新物体状态为动态，计算物体在地平面上的运动速度；如果6帧范围内的平移不超过阈值，则更新物体状态为静态。
与现有技术相比，本发明具备的优势为：
1)本发明提出了一种面向自动驾驶场景的双目语义SLAM方法，该方法能够在跟踪相机自身运动的同时，利用神经网络检测图像中的物体，得到包含物体类别的包围框、掩膜等更高级的视觉语义信息，对环境中感兴趣的目标进行检测和持续跟踪，根据物体跟踪的结果可以判断其是否为动态物体，并将信息反馈到 SLAM系统中，提升高动态场景下跟踪的精度和鲁棒性。
2)本发明使用二次曲面作为三维物体的表达，与之前的方法仅仅将二次曲面用作对静态物体建图或者做路标相比，本发明无论是对于场景中的静态物体，还是运动中的物体，都可以用二次曲面表达不同时刻物体的状态。将二次曲面约束为椭球面后，椭球面的参数可以表示物体的位姿和尺寸，具有直观的物理意义，而且将椭球面投影到相机平面上所得的椭圆，可以方便地与目标检测算法得到的二维包围框建立约束，使得物体的状态易于求解。
3)本发明针对语义物体观测不足的情况，充分利用神经网络和先验信息进行约束，即除了椭球面投影与二维包围框的约束构建的误差项外，又增加了物体的朝向误差、尺寸误差项，实现了对物体状态参数的非线性优化求解，这样不仅可以为物体参数提供一个较好的初值，而且可以保证优化过程快速收敛。
附图说明
图1是本发明的SLAM方法流程图；
图2是本发明在KITTI raw data部分动态场景序列下测试的结果；
图3是在高速公路上对车辆的跟踪效果图；
图4是在复杂的城镇道路上对车辆的跟踪效果图。
具体实施方式
下面结合说明书附图对本发明进行详细说明。本发明中各个实施方式的技术特征在没有相互冲突的前提下，均可进行相应组合。
本发明的面向自动驾驶场景的双目语义SLAM方法，如图1所示，包括如下步骤：
步骤1：通过神经网络检测图像中的语义物体，得到物体的二维包围框、掩膜及朝向；
步骤2：通过ORB特征点法对双目相机进行跟踪，得到相机的位姿；
步骤3：使用二次曲面表达语义物体，利用特征点、语义信息、先验信息不断跟踪和优化得到物体不同时刻的位姿；
步骤4：利用物体最近的跟踪结果为当前的特征点提供先验信息，排除动态物体干扰，提高相机跟踪的精度和鲁棒性。
在本发明的一项具体实施中，对步骤1的实施过程进行了介绍。
使用目标检测网络，对左目图像中的物体进行检测，得到物体的二维包围框，然后对检测到的物体使用朝向预测网络，预测每个物体的朝向先验，而后使用实例分割网络，将二维包围框作为原有实例分割网络中区域提议网络部分的输出，输入到后续网络中，得到每个物体的掩膜。其中，目标检测网络和朝向预测网络基于[Garrick Brazil,XiaomingLiu.M3d-rpn:Monocular 3d region proposal network for object detection.InProceedings of the IEEE International Conference on Computer Vision.2019,9287-9296.]中的方法，实例分割网络使用[Kaiming He, Georgia Gkioxari,Piotr Dollár,Ross Girshick.Mask r-cnn.In Proceedings of theIEEE international conferenceon computer vision.2017,2961-2969.]所述的方法。
在本发明的一项具体实施中，对步骤2的实施过程进行了介绍。
首先对系统进行初始化。系统接收双目图像流后，对输入的图像进行处理，如果是彩色图像，则转化为灰度图，随后使用图像构造当前帧。当前帧构造时，首先同时对左目和右目图像进行ORB特征点提取，然后根据神经网络生成的掩膜将左目图像中的特征点标记为属于某个物体或属于背景，接着进行双目特征点匹配。
当前帧构造完成后尝试进行跟踪的初始化，如果图像提取的特征点足够丰富，且双目图像直接特征点匹配足够多，则利用三角化生成初始的地图点，初始化完成。否则，利用后续帧继续尝试初始化。
初始化完成后，系统开始对双目相机跟踪。用来表示t时刻相机的状态，其中c代表相机，w表示世界坐标系，那么有/>是相机的6自由度位姿，而t时刻相机对路标/>的观测用/>来表示，那么要求解的就是给定0到T时刻所有相机的观测/>求解0到T时刻所有相机的状态/>和观测到的部分路标的位置/>
假设观测噪声服从高斯分布，那么可以通过极大似然估计(MLE)求解：
残差r构建为重投影误差，由于是双目相机，残差项有两个，即：
若跟踪当前帧时相机有记录的速度，则首先根据上一帧与上一帧参考帧的相对位姿更新上一帧的位姿，然后用恒速模型预测当前帧的位姿。匹配完成后，做一个3D-2D的位姿估计，得到当前帧位姿。而后在与局部地图中的点进行匹配，进一步做一个3D-2D的位姿优化。若恒速模型还未生成，比如刚初始化完成，或者刚刚完成重定位，则需要和参考关键帧匹配，匹配使用词袋[Dorian Gálvez-López,Juan D Tardos.Bags of binary wordsfor fast place recognition in imagesequences.IEEE Transactions on Robotics,2012,28(5):1188-1197.]进行加速，匹配完成后同样再与局部地图匹配，最终优化得到当前帧位姿。如果跟踪失败，则进行重定位。
跟踪完成后，会判断是否需要生成关键帧。生成的关键帧会传给建图线程。后端建图时，局部建图线程首先将新的关键帧加入共视图(Co-visibility Graph) 中，更新有共视关系的关键帧的边，更新生成树，并计算新关键帧的词袋向量。然后剔除冗余的地图点，并三角化生成新的地图点。剔除地图点时，会对已标记为动态的地图点进行剔除，同时生成新地图点时也会跳过被标记为动态的特征点。随后，对一定窗口范围内的关键帧和地图点进行局部集束调整(Local BA)，优化关键帧的位姿和地图点的位置。
系统会不断进行回环检测，用来减少累计误差。回环检测时，系统会检测当前关键帧及共视图中附近的关键帧，计算词袋分数，将分数最低的一个关键帧与识别数据库进行比较，若相似性达到一定阈值，则将当前关键帧及其相邻两帧、闭环候选关键帧及其相邻两帧取出来进行交叉验证，防止错误的闭环匹配。匹配成功后，使用相似变换群对当前关键帧的位姿进行矫正，然后纠正相邻关键帧及观察到的地图点，之后对先对本质图(EssentialGraph)进行优化，最后使用全局集束调整(Global BA)对所有帧的位姿和地图点进行优化。
在本发明的一项具体实施中，对步骤3的实施过程进行了介绍。
在本发明中，使用二次曲面来表达三维物体。严格来讲，本实施例使用的是二次曲面中的椭球面来表示，因为只有封闭的二次曲面对于物体表达才有意义。二次曲面在射影几何中具有较好的形式，将其参数解耦后可以通过限定参数的取值使其约束为椭球面，解耦后的椭球面参数具有9个自由度，可以直接用来表示物体的尺寸、位置、朝向，物理意义直观。椭球面在成像平面上的投影轮廓是一个椭圆，可以自然地与二维包围框形成约束关系。现有技术中采用二维曲面进行的工作仅仅将其作为路标，或者地图的一部分来辅助定位，而本发明利用二次曲面求得不同时刻物体的位姿，即可以用二次曲面来跟踪物体。
使用二次曲面作为物体的表达方式，具体来说，将二次曲面Q的对偶形式Q*进行分解，有：
Q*＝UD*UT
其中U是实正交矩阵，D*是实对角矩阵。通过对D*进行约束，使得Q*被约束成为一个椭球面。具体来说，如果使得D*前三个对角线元素为正，最后一个对角线元素为-1，那么D*以及Q*就被约束为椭球面。记被约束为椭球面的D*为那么椭球面就可以表示为：
其中，R是三维空间中的旋转矩阵，t是三维空间中的平移向量，s1，s2，s3是椭球面的三个半轴。这样分解后的矩阵参数都具有直观的物理意义，表示半轴长分别为s1，s2，s3的椭球面，通过欧式变换U变换到相应的姿态。
首先对每帧图像中检测到的物体使用网络提供的朝向先验和预设不同物体的尺寸先验进行初始化，即使用朝向先验和姿态约束(物体平行于地面)构造R，使用物体的尺寸作为中s1，s2，s3的值，然后使用物体y轴方向(相机坐标系) 的尺寸与其图像中二维包围框纵向的高度(像素)，以及双目相机离地面的高度，计算粗略的t的初始值，这样就得到了物体的初始化参数。
然后需要对物体进行数据关联，即得到同一个物体在不同帧的对应关系。本发明不同帧物体之间的数据关联使用特征点匹配进行。对于上一帧的每个物体，在当前帧中相同位置附近的一定范围内从最近的物体开始特征匹配；
若上一帧匹配成功，则将匹配的特征点数量最多的候选物体认为是同一个物体，沿用上一帧中物体的运动状态，更新物体ID和物体位姿，并将当前帧中的物体与上一帧中的物体同时优化，更新物体的绝对位姿；
若匹配不成功，则寻找参考帧进行匹配，将匹配成功的参考帧中的物体认为是同一个物体，沿用参考帧中物体的运动状态，更新物体ID和物体位姿，并将当前帧中的物体与参考帧中的物体同时优化，更新物体的绝对位姿；若参考帧匹配不成功，则当前帧中的物体为新的物体，对新的物体进行优化，更新物体的绝对位姿。
用表示t时刻第k个物体的状态，其中o代表物体，w表示世界坐标系，那么有是物体的6自由度位姿。但对于物体来说，还有物体的三维尺寸/>是未知的，同样需要求解，那么未知参数就一共有9个自由度。而t时刻对物体的观测用/>来表示，三维物体跟踪问题要求解的就是给定0到T时刻所有对物体的语义观测以及对物体所属特征点的观测/>和相机的状态/>求解 0到T时刻所有物体的状态/>
将求解问题用概率分布的形式写出来，有：
假设语义观测和特征点观测，以及特征点观测和物体的尺寸信息是相互独立的，上式就变为：
P(wxok，dok|bk，uok，wxc)∝P(bk|dok，wxok，wxc)P(uok|xok，wxc)P(wxok)P(dok)
通常，没有先验的情况下，可以通过极大似然估计求解：
但实际上，这样求解是十分困难的，因为对本发明所针对的自动驾驶场景来说，相机对于物体的观测是十分不足的：一是观测时间十分有限，通常物体从出现在视野内到消失可能只有很短的时间；二是观测角度十分有限，相机只能随着车辆载体运动，而车辆运动的维度十分受限；三是物体本身就容易受到环境中其他物体的遮挡，这也会影响对物体的观测。观测十分受限的数据用优化问题求解往往难以收敛，优化结果甚至根本无法使用，所以本实施例中考虑加入先验的约束。加入先验信息后，将其转化为一个最大后验估计问题：
将该问题变为一个非线性优化问题：
这样就可以分别构建4个残差项用于优化求解。
具体来说，对于rb残差，要构建的误差项是当前描述物体位姿的椭球面投影到图像后的椭圆的外接包围框，和输入包围框的误差，使用左上、右下两个点共 4个坐标值的误差表示。所构成的残差为：
式中adj(·)表示二次曲线的外接包围框，表示相机的内参、外参构成的投影矩阵，/>表示由dok和/>为参数构造的对偶二次曲面。
椭圆的外接包围框可以由对偶二次曲线的定义lTC*l＝0来计算，若 l＝[a,b,c]T，令b＝0，则可求得包围框在x轴上的最大值和最小值，令a＝0，则可求得包围框在y轴上的最大值和最小值。
对于ru残差，用表示属于物体k的第m个特征点相对于物体的坐标，则有：
对于位姿先验，实际上只包含一维的朝向先验，而且网络预测得到的朝向都是相对于当前相机坐标系下的，所以计算前需要物体朝向转到当前相机坐标系下。用 o表示朝向向量，使用物体朝向和网络预测朝向向量夹角的余弦来计算残差有：
尺寸先验的残差为：
构建好残差后，使用非线性优化的方法即可迭代求解。本发明使用ceres非线性优化库作为求解工具。
在本发明的一项具体实施中，对步骤4的实施过程进行了介绍。
对物体进行跟踪后，会根据它在世界坐标系下的平移变化量做出判断。判断采用滑动窗口形式，记录物体最近6帧范围内的平移，如果物体在3帧范围内出现较大平移，则判断它为动态物体，然后更新相应的变量，计算物体在地平面上的运动速度。但是由于输入噪声的影响，尤其是被遮挡物体的检测噪声较大，个别静态的物体可能会被错判为动态，而且环境中也可能出现由运动变为静止的物体，所以又加入了反向判断机制，如果滑动窗口的6帧内物体总体上没有较大的平移，则判断为静态物体。这样的机制也可以对环境中动静变化的物体做出判断。物体的运动状态改变后，会更新相应的变量，将物体关联的特征点，以及相应的地图点标记为动态或者静止，标记为动态的特征点不会被生成新的地图点，而标记为动态的地图点不会参与相机的跟踪，也不会参与局部集束调整，直接会被视作外点，在后续的跟踪中被剔除。
由于物体的跟踪是在相机跟踪之后进行的，动态物体的判断会有些“滞后”，因此将物体的匹配步骤置于相机跟踪之前，这样在上一帧中的物体状态会提前反馈到当前帧中，属于动态物体的点就可以在跟踪之前更新，这样相机跟踪时就能及时排除动态特征点，从而优化跟踪结果。
实施例
为了进一步展示本发明的实施效果，本发明从KITTI的原始数据(Raw Data) 中挑选了8个动态车辆较多的场景进行测试，测试使用数据集提供的彩色双目图像，因KITTI没有提供原始数据的Ground Truth，本实施例使用与KITTI中相同的方法，将高精惯导数据生成的位姿投影到矫正后左目彩色相机坐标系下的结果作为Ground Truth。为了使物体匹配更加稳定，测试中将ORB-SLAM2和本发明工作的特征点提取数量均设为5000。由于回环检测会减少累计误差，为了更加准确地评估本发明方法对误差的影响，评估时均关闭回环检测进行测试。结果使用Grupp等人的评估工具评估相对位姿误差(Relative Pose Error，RPE)和绝对轨迹误差(Absolute Trajectory Error，ATE)。测试结果如表1。
表1 KITTI raw data数据测试结果
可以看到，对于动态物体较多的场景，本发明的方法相比于原有的 ORB-SLAM2展现出了较大的优势，尤其是动态车辆非常多的1003_0047序列，精度提升十分明显。该实验充分说明，本发明使用的方法对于SLAM系统在高动态场景下的精度表现提升十分明显。
通过实施效果图可以直观地展示本发明的实施效果。图2中被检测到的车辆都被所示的椭球面表示，二维包围框为目标检测的结果，动态物体和静态物体可以通过不同颜色的二维包围框表示。在本实施例中，为了直观的说明效果，在对应的物体旁边采用“静止”和“动态”进行了区分。包围框上方为物体ID，动态物体右下角会显示动态物体当前的大致速度。
图3为高速公路中对同一动态车辆的跟踪效果，同样可以看到图中随着车辆的前进可以被稳定跟踪，并且实时显示其当前速度，图中首尾帧间隔100帧左右。
图4为城镇中更复杂环境的跟踪效果，图中显示的这段序列中视野前方有一动态车辆(ID为1135)，而道路两侧都是静态车辆，可以看出，系统能够判断场景中物体是否动态，并同时分别进行跟踪。图中首尾帧间隔约280帧，即使间隔较长，视野前方的动态车辆也一直能被持续、稳定地跟踪，足以证明本发明提出的物体跟踪方法可以较为鲁棒地跟踪环境中的物体。
以上列举的仅是本发明的具体实施例。显然，本发明不限于以上实施例，还可以有许多变形。本领域的普通技术人员能从本发明公开的内容直接导出或联想到的所有变形，均应认为是本发明的保护范围。
Claims (8)
1.一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，包括如下步骤：
1)通过神经网络模型检测左目图像中的物体，得到物体的二维包围框、掩膜及朝向；
2)采用ORB特征点法提取双目图像的特征点，根据神经网络得到的掩膜赋予特征点先验信息，利用属于背景的特征点以及物体跟踪后属于静态物体的特征点对双目相机进行跟踪，得到相机的位姿，并生成关键帧，然后将关键帧信息传输到后端进行建图和回环检测；
3)使用二次曲面表达物体，采用特征点匹配的方式关联前后帧物体，利用二维包围框和二次曲面投影之间的约束以及先验信息提供的约束，不断跟踪和优化得到物体不同时刻的相对位姿，而后利用相机跟踪结果得到物体的绝对位姿，最后利用物体跟踪的结果为下一帧的特征点提供先验信息，提高相机跟踪的精度和鲁棒性；
所述的步骤3)包括如下步骤：
3.1)使用二次曲面表达物体，进行物体初始化；所述的二次曲面表达物体具体为：
其中，Q*为二次曲面的对偶形式，是实正交矩阵，表示半轴长分别为s1，s2，s3的椭球面，使用物体的尺寸先验作为1，s2，s3的值；使用朝向先验和姿态约束构造R，R是三维空间中的旋转矩阵，t是三维空间中的平移向量；
使用物体在相机坐标系下y轴方向的尺寸与其图像中二维包围框纵向的高度(像素)，以及双目相机离地面的高度计算t的初始值，得到物体的初始化参数；
3.2)对物体进行特征点关联：
对于上一帧中的每个物体，在当前帧中相同位置区域内从最近的物体开始进行特征点匹配；
若特征点的匹配数量多于预设值，则物体匹配成功，匹配的特征点数量最多的候选物体认为与上一帧中相匹配的物体是同一个物体，更新该物体的运动状态为该物体在上一帧中的运动状态，更新物体ID和物体位姿，并将当前帧中的物体与上一帧中的物体同时优化，更新物体的绝对位姿；
若特征点匹配数量少于预设值，则认为匹配不成功，需寻找参考帧进行匹配，将匹配成功的参考帧中的物体认为是同一个物体，沿用参考帧中物体的运动状态，更新物体ID和物体位姿，并将当前帧中的物体与参考帧中的物体同时优化，更新物体的绝对位姿；若与参考帧中物体匹配的特征点仍然少于预设值，即匹配不成功，则当前帧中的物体为新的物体，对新的物体进行优化，更新物体的绝对位姿；
3.3)判断物体的运动状态，若物体的运动状态发生改变，则将物体关联的特征点、以及相应的地图点标记为动态或者静止，标记为动态的特征点不会被生成新的地图点，标记为动态的地图点不会参与相机的跟踪，也不会参与局部集束调整，直接会被视作外点，在后续的相机跟踪中被剔除。
2.如权利要求1所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，所述的步骤1)包括如下步骤：
1.1)使用目标检测网络，对左目图像中的物体进行检测，得到物体的二维包围框；
1.2)对检测到的物体使用朝向预测网络，预测每个物体的朝向先验；
1.3)使用实例分割网络，将物体的二维包围框作为实例分割网络中区域提议网络部分的输出，得到每个物体的掩膜先验。
3.如权利要求1所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，所述的步骤2)包括如下步骤：
2.1)双目初始化；
2.2)初始化成功后，对双目相机进行跟踪，采用恒速模型预测当前帧位姿，并对当前帧位姿进行3D-2D的位姿优化；而后再与局部地图中的地图点进行匹配，并做3D-2D的位姿优化，最终得到优化后的当前帧位姿；
2.3)若跟踪过程中有效匹配的特征点低于阈值，则认为跟踪失败，进入重定位流程；若跟踪成功，则更新物体的绝对位姿并生成关键帧，由生成关键帧进行建图，并不断进行回环检测。
4.如权利要求3所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，刚完成双目初始化时，恒速模型还未生成，此时需要先将当前帧与参考关键帧匹配，再与局部地图匹配，最终优化得到当前帧位姿。
5.如权利要求3所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，所述的步骤2.1)具体为：
2.1.1)接收双目图像流，分别对左目图像和右目图像进行灰度图处理；
2.1.2)对当前帧的左目图像和右目图像进行ORB特征点提取，然后根据神经网络生成的掩膜先验，将左目图像中的特征点标记为属于某个物体或属于背景；
2.1.3)进行双目特征点匹配，若双目特征点匹配的数量大于阈值，则利用三角化生成初始的地图点，双目初始化完成；否则，重复步骤2.1.2)至步骤2.1.3)，用后续帧继续进行初始化，直至双目初始化完成。
6.如权利要求3所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，步骤2.3)所述的由生成关键帧进行建图，并不断进行回环检测，具体为：
2.3.1)建图过程中，首先将新的关键帧插入到共视图中，更新有共视关系的关键帧的边及生成树，计算新的关键帧的词袋向量；
2.3.2)剔除冗余的地图点，利用三角化生成新的地图，并对窗口范围内的关键帧和地图点进行局部集束调整，优化关键帧的位姿和地图点的位置，随后进行关键帧剔除，若某一关键帧90％以上地图点能够被其他3个以上关键帧观测到，则剔除该关键帧；
2.3.3)进行回环检测，计算当前关键帧及共视图中附近的关键帧的相似度得分，将分数最低的一个关键帧与识别数据库中的闭环候选关键帧进行比较，若相似性达到一定阈值，则对当前关键帧及其相邻两帧、闭环候选关键帧及其相邻两帧进行交叉验证；根据交叉验证结果，首先使用相似变换群对当前关键帧的位姿进行矫正，再纠正相邻关键帧以及相邻关键帧能够观察到的地图点，之后对本质图进行优化，最后再使用全局集束调整对所有关键帧的位姿和地图点进行优化。
7.如权利要求6所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，所述步骤2.3.2)中剔除冗余的地图点时，对已标记为动态的地图点进行剔除，同时生成新地图点时也会跳过被标记为动态的特征点。
8.如权利要求1所述的一种面向自动驾驶场景的双目语义SLAM方法，其特征在于，物体的运动状态分为动态或者静止，判断物体运动状态的方法为：
首先将所有物体的状态初始化为动态，然后对物体在世界坐标系下的平移变化量进行判断，采用滑动窗口形式，记录物体最近6帧范围内的平移，如果物体在3帧范围内平移超过阈值，则更新物体状态为动态，计算物体在地平面上的运动速度；如果6帧范围内的平移不超过阈值，则更新物体状态为静态。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010922292.XA CN112101160B (zh) | 2020-09-04 | 2020-09-04 | 一种面向自动驾驶场景的双目语义slam方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010922292.XA CN112101160B (zh) | 2020-09-04 | 2020-09-04 | 一种面向自动驾驶场景的双目语义slam方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112101160A CN112101160A (zh) | 2020-12-18 |
| CN112101160B true CN112101160B (zh) | 2024-01-05 |
Family
ID=73757784
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010922292.XA Active CN112101160B (zh) | 2020-09-04 | 2020-09-04 | 一种面向自动驾驶场景的双目语义slam方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112101160B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112348029B (zh) * | 2021-01-08 | 2021-08-10 | 禾多科技（北京）有限公司 | 局部地图调整方法、装置、设备和计算机可读介质 |
| CN113158816B (zh) * | 2021-03-29 | 2024-02-20 | 东北大学 | 面向室外场景物体的视觉里程计二次曲面路标构建方法 |
| CN113052907B (zh) * | 2021-04-12 | 2023-08-15 | 深圳大学 | 一种动态环境移动机器人的定位方法 |
| CN113160315B (zh) * | 2021-04-16 | 2023-01-20 | 广东工业大学 | 一种基于对偶二次曲面数学模型的语义环境地图表征方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110349250A (zh) * | 2019-06-28 | 2019-10-18 | 浙江大学 | 一种基于rgbd相机的室内动态场景的三维重建方法 |
| CN110458896A (zh) * | 2019-08-07 | 2019-11-15 | 成都索贝数码科技股份有限公司 | 一种基于绝对二次曲面的相机内参求解方法及系统 |
| CN111402336A (zh) * | 2020-03-23 | 2020-07-10 | 中国科学院自动化研究所 | 基于语义slam的动态环境相机位姿估计及语义地图构建方法 |
| CN111429494A (zh) * | 2020-04-13 | 2020-07-17 | 中国空气动力研究与发展中心超高速空气动力研究所 | 一种基于生物视觉的点云高精度自动配准方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11199414B2 (en) * | 2016-09-14 | 2021-12-14 | Zhejiang University | Method for simultaneous localization and mapping |
-
2020
- 2020-09-04 CN CN202010922292.XA patent/CN112101160B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110349250A (zh) * | 2019-06-28 | 2019-10-18 | 浙江大学 | 一种基于rgbd相机的室内动态场景的三维重建方法 |
| CN110458896A (zh) * | 2019-08-07 | 2019-11-15 | 成都索贝数码科技股份有限公司 | 一种基于绝对二次曲面的相机内参求解方法及系统 |
| CN111402336A (zh) * | 2020-03-23 | 2020-07-10 | 中国科学院自动化研究所 | 基于语义slam的动态环境相机位姿估计及语义地图构建方法 |
| CN111429494A (zh) * | 2020-04-13 | 2020-07-17 | 中国空气动力研究与发展中心超高速空气动力研究所 | 一种基于生物视觉的点云高精度自动配准方法 |
Non-Patent Citations (2)
| Title |
|---|
| Survey and evaluation of monocular visual-inertial SLAM algorithms for augmented reality;Li Jinyu 等;ScienceDirect;第4卷(第1期);全文 * |
| 基于生成对抗网络的图像恢复与SLAM容错研究;王凯 等;浙江大学学报(工学版);第53卷(第1期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112101160A (zh) | 2020-12-18 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Yang et al. | Cubeslam: Monocular 3-d object slam | |
| CN111462135B (zh) | 基于视觉slam与二维语义分割的语义建图方法 | |
| CN112101160B (zh) | 一种面向自动驾驶场景的双目语义slam方法 | |
| CN111156984B (zh) | 一种面向动态场景的单目视觉惯性slam方法 | |
| CN108051002A (zh) | 基于惯性测量辅助视觉的运输车空间定位方法及系统 | |
| CN110717927A (zh) | 基于深度学习和视惯融合的室内机器人运动估计方法 | |
| CN112734841B (zh) | 一种用轮式里程计-imu和单目相机实现定位的方法 | |
| Ding et al. | Vehicle pose and shape estimation through multiple monocular vision | |
| CN111046856B (zh) | 基于动静态特征提取并行位姿跟踪与地图创建的方法 | |
| Yin et al. | Dynam-SLAM: An accurate, robust stereo visual-inertial SLAM method in dynamic environments | |
| CN111753696B (zh) | 一种感知场景信息的方法、仿真装置、机器人 | |
| CN110070578B (zh) | 一种回环检测方法 | |
| CN109781092A (zh) | 一种危险化工事故中移动机器人定位与建图方法 | |
| CN106846367B (zh) | 一种基于运动约束光流法的复杂动态场景的运动物体检测方法 | |
| Yang et al. | CubeSLAM: Monocular 3D object detection and SLAM without prior models | |
| CN114708293A (zh) | 基于深度学习点线特征和imu紧耦合的机器人运动估计方法 | |
| Dong et al. | A novel texture-less object oriented visual SLAM system | |
| CN117523461B (zh) | 一种基于机载单目相机的运动目标跟踪与定位方法 | |
| Ge et al. | Vipose: Real-time visual-inertial 6d object pose tracking | |
| CN114549549A (zh) | 一种动态环境下基于实例分割的动态目标建模跟踪方法 | |
| Yoon et al. | A new approach to the use of edge extremities for model-based object tracking | |
| Coenen et al. | Detection and 3d modelling of vehicles from terrestrial stereo image pairs | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN116643291A (zh) | 一种视觉与激光雷达联合剔除动态目标的slam方法 | |
| Jiang et al. | Icp stereo visual odometry for wheeled vehicles based on a 1dof motion prior |
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 |