CN114119763A - 一种用于自动驾驶车辆的Lidar标定方法及装置 - Google Patents

一种用于自动驾驶车辆的Lidar标定方法及装置 Download PDF

Info

Publication number
CN114119763A
CN114119763A CN202010894839.XA CN202010894839A CN114119763A CN 114119763 A CN114119763 A CN 114119763A CN 202010894839 A CN202010894839 A CN 202010894839A CN 114119763 A CN114119763 A CN 114119763A
Authority
CN
China
Prior art keywords
camera
image
lidar
pose
target
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010894839.XA
Other languages
English (en)
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.)
Beijing Momenta Technology Co Ltd
Original Assignee
Beijing Chusudu Technology 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 Beijing Chusudu Technology Co ltd filed Critical Beijing Chusudu Technology Co ltd
Priority to CN202010894839.XA priority Critical patent/CN114119763A/zh
Publication of CN114119763A publication Critical patent/CN114119763A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • Image Analysis (AREA)

Abstract

本发明实施例公开一种用于自动驾驶车辆的Lidar标定方法及装置。应用本发明实施例提供的方案，能够对多相机模组进行标定，得到各相机的内参以及各其他相机相对于主相机的外参后，基于标定得到的相机内参和外参，对各相机图像进行三维重建，得到各三维点集合，并且，可以将Lidar点云数据投影至二维空间得到Lidar深度图，从而可以根据三维点集合和Lidar深度图构建关于由多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，对函数进行优化得到多相机模组到Lidar的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。

Description

一种用于自动驾驶车辆的Lidar标定方法及装置
技术领域
本发明涉及自动驾驶技术领域，具体而言，涉及一种用于自动驾驶车辆的Lidar标定方法及装置。
背景技术
在自动驾驶的多传感器方案中，如何方便而准确的标定传感器的内参，以及传感器之间的外参一直是一个富有挑战性的问题。
例如，在自动驾驶车辆安装有多个相机和Lidar(Light Detection And Ranging，激光雷达)的情况下，需要标定得到多个相机组成的多相机模组到Lidar的外参，才可以准确的对各传感器采集的数据进行处理，保证车辆安全行驶，因此，亟需一种Lidar标定方法。
发明内容
本发明提供了一种用于自动驾驶车辆的Lidar标定方法及装置，以对自动驾驶车辆进行Lidar标定。具体的技术方案如下。
第一方面，本发明实施例提供一种用于自动驾驶车辆的Lidar标定方法，所述自动驾驶车辆安装有多个相机，所述方法包括：
获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参；所述主相机为所述各相机中任一相机；
根据各所述相机的内参以及所述各其他相机相对于所述主相机的外参，对各所述相机图像进行特征点的检测与匹配，得到各所述相机图像的目标位姿；
针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧；
针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息；并根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合；
获取Lidar点云数据，在所述Lidar点云数据中识别时间戳与各所述关键帧的采集时间对应的关键Lidar点云数据；并根据Lidar测量模型，将所述关键Lidar点云数据投影至二维空间，并进行双线性插值，得到各Lidar深度图；
根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参。
可选的，所述根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参的步骤包括：
根据以下公式对所述多相机模组到Lidar的外参
Figure BDA0002658138680000021
进行优化：
Figure BDA0002658138680000022
πl(·)表示所述Lidar测量模型，Ii(x)表示i时刻对应的Lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，Vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
可选的，所述根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参的步骤包括：
根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
针对每张所述相机图像，提取该相机图像中的关键点，并根据所提取的关键点确定该相机图像的全局描述子；
获取预先构建的特征地图，所述特征地图包括多帧图像，各所述图像包括各关键点和对应的描述子；针对每张相机图像，在所述特征地图中检索与该相机图像的全局描述子相似度最高的描述子对应的图像，作为该相机图像对应的匹配图像；
将各所述相机图像和对应的匹配图像中的关键点进行匹配，并针对每张相机图像，随机采样该相机图像和对应的匹配图像中预设数量对关键点，根据采样得到的关键点计算该相机图像相对所述特征地图的位姿；
确定各所述相机中的主相机，针对每个其他相机，根据该相机所采集的各相机图像相对所述特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿；
根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参；
根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
可选的，所述根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参的步骤包括：
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
针对每个其他相机，确定该相机相对所述主相机的初始外参为，该相机对应的目标相机图像的初始位姿，乘以与该目标相机图像同图像集的主相机坐标系的逆。
可选的，所述根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参的步骤包括：
根据以下公式对各相机的内参进行优化：
Figure BDA0002658138680000041
Figure BDA0002658138680000042
表示j时刻世界系到相机i的坐标变换，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure BDA0002658138680000043
是由相机内参θi决定的相机的投影模型；
针对每个所述其他相机，根据以下公式对该相机相对于所述主相机的外参进行优化：
Figure BDA0002658138680000044
Figure BDA0002658138680000045
表示j时刻主相机h的坐标系，
Figure BDA0002658138680000046
表示相机i相对于主相机h的外参。
第二方面，本发明实施例提供一种用于自动驾驶车辆的Lidar标定装置，所述自动驾驶车辆安装有多个相机，所述装置包括：
相机标定模块，用于获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参；所述主相机为所述各相机中任一相机；
目标位姿确定模块，用于根据各所述相机的内参以及所述各其他相机相对于所述主相机的外参，对各所述相机图像进行特征点的检测与匹配，得到各所述相机图像的目标位姿；
关键帧确定模块，用于针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧；
深度估计模块，用于针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息；并根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合；
数据投影模块，用于获取Lidar点云数据，在所述Lidar点云数据中识别时间戳与各所述关键帧的采集时间对应的关键Lidar点云数据；并根据Lidar测量模型，将所述关键Lidar点云数据投影至二维空间，并进行双线性插值，得到各Lidar深度图；
外参标定模块，用于根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参。
可选的，所述外参标定模块，具体用于：
根据以下公式对所述多相机模组到Lidar的外参
Figure BDA0002658138680000051
进行优化：
Figure BDA0002658138680000052
πl(·)表示所述Lidar测量模型，Ii(x)表示i时刻对应的Lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，Vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
可选的，所述相机标定模块包括：
图像同步子模块，用于根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
描述子确定子模块，用于针对每张所述相机图像，提取该相机图像中的关键点，并根据所提取的关键点确定该相机图像的全局描述子；
图像匹配子模块，用于获取预先构建的特征地图，所述特征地图包括多帧图像，各所述图像包括各关键点和对应的描述子；针对每张相机图像，在所述特征地图中检索与该相机图像的全局描述子相似度最高的描述子对应的图像，作为该相机图像对应的匹配图像；
位姿计算子模块，用于将各所述相机图像和对应的匹配图像中的关键点进行匹配，并针对每张相机图像，随机采样该相机图像和对应的匹配图像中预设数量对关键点，根据采样得到的关键点计算该相机图像相对所述特征地图的位姿；
初始位姿确定子模块，用于确定各所述相机中的主相机，针对每个其他相机，根据该相机所采集的各相机图像相对所述特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿；
初始外参计算子模块，用于根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参；
相机标定子模块，用于根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
可选的，所述初始外参计算子模块，具体用于：
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
针对每个其他相机，确定该相机相对所述主相机的初始外参为，该相机对应的目标相机图像的初始位姿，乘以与该目标相机图像同图像集的主相机坐标系的逆。
可选的，所述相机标定子模块，具体用于：
根据以下公式对各相机的内参进行优化：
Figure BDA0002658138680000061
Figure BDA0002658138680000062
表示j时刻世界系到相机i的坐标变换，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure BDA0002658138680000063
是由相机内参θi决定的相机的投影模型；
针对每个所述其他相机，根据以下公式对该相机相对于所述主相机的外参进行优化：
Figure BDA0002658138680000071
Figure BDA0002658138680000072
表示j时刻主相机h的坐标系，
Figure BDA0002658138680000073
表示相机i相对于主相机h的外参。
由上述内容可知，本发明实施例提供的用于自动驾驶车辆的Lidar标定方法及装置，能够对多相机模组进行标定，得到各相机的内参以及各其他相机相对于主相机的外参后，基于标定得到的相机内参和外参，对各相机图像进行三维重建，得到各三维点集合，并且，可以将Lidar点云数据投影至二维空间得到Lidar深度图，从而可以根据三维点集合和Lidar深度图构建关于由多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，对函数进行优化得到多相机模组到Lidar的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。当然，实施本发明的任一产品或方法并不一定需要同时达到以上所述的所有优点。
本发明实施例的创新点包括：
1、能够对多相机模组进行标定，得到各相机的内参以及各其他相机相对于主相机的外参后，基于标定得到的相机内参和外参，对各相机图像进行三维重建，得到各三维点集合，并且，可以将Lidar点云数据投影至二维空间得到Lidar深度图，从而可以根据三维点集合和Lidar深度图构建关于由多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，对函数进行优化得到多相机模组到Lidar的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。
2、能够基于预先构建的特征地图确定各相机采集的相机图像的位姿，由于特征地图是固定不变的，各相机图像的位姿即可反映各相机之间的位置关系，因此能够基于各相机图像的位姿计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客观准确，从而能够提高相机标定的准确性。并且，基于特征地图对相机进行标定时，只需要预先构建得到特征地图即可，不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单介绍。显而易见地，下面描述中的附图仅仅是本发明的一些实施例。对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的用于自动驾驶车辆的Lidar标定方法的一种流程示意图；
图2为本发明实施例提供的用于自动驾驶车辆的Lidar标定方法的另一种流程示意图；
图3为本发明实施例提供的用于自动驾驶车辆的Lidar标定装置的一种结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述。显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，本发明实施例及附图中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。例如包含的一系列步骤或单元的过程、方法、***、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。
本发明实施例公开了一种用于自动驾驶车辆的Lidar标定方法及装置，能够对自动驾驶车辆进行Lidar标定。下面对本发明实施例进行详细说明。
图1为本发明实施例提供的用于自动驾驶车辆的Lidar标定方法的一种流程示意图。该方法应用于自动驾驶车辆，该自动驾驶车辆安装有多个相机，该方法具体包括以下步骤。
S110：获取各相机采集的相机图像，以及各相机图像对应的采集时刻；根据各相机图像计算各相机的内参以及各其他相机相对于主相机的外参；主相机为各相机中任一相机。
在本发明实施例中，进行Lidar标定时，数据输入为各相机采集的相机图像，以及Lidar采集的Lidar点云数据，数据输出即为多相机模组到Lidar的外参。其中，各相机图像标注有对应的时间戳，也就是各相机图像的采集时刻，Lidar点云数据也标注有对应的时间戳。
获取到各相机图像后，可以确定其中任一相机为主相机。并且，可以根据各相机图像计算各相机的内参以及各其他相机相对于主相机的外参，例如，可以采用任一种已知的方法根据各相机图像来计算各相机的内参以及各其他相机相对于主相机的外参，本发明实施例对此不作限定。
S120：根据各相机的内参以及各其他相机相对于主相机的外参，对各相机图像进行特征点的检测与匹配，得到各相机图像的目标位姿。
在一种实现方式中，可以将各相机的内参、各其他相机相对于主相机的外参、以及各相机图像输入到一个多相机视觉里程计***中，此***通过视觉特征点的检测与匹配，计算出每一帧相机图像的目标位姿。
S130：针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧。
在一种实现方式中，可以由多相机视觉里程计***在计算出每帧相机图像的目标位姿后，选取所有相机图像中距离与角度间隔足够大的帧作为关键帧。其中，上述预设距离阈值和预设角度阈值均可以根据实际情况确定，本发明实施例对此不作限定。
针对同一时刻不同相机采集的相机图像，当确定任一相机图像为关键帧时，即可将该时刻所有相机采集的相机图像均确定为关键帧。
S140：针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息，并根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合。
在一种实现方式中，可以将关键帧的目标位姿与相机图像输入到视觉实时稠密重建***中，此***选取图像中有梯度的点进行深度估计，输出每一帧的视觉三维点信息，可以称为局部三维点信息。
可以理解，存在采集时刻相同的关键帧。因此，在本发明实施例中，得到各关键帧的局部三维点信息后，可以对各关键帧的局部三维点信息进行统计，得到各采集时刻对应的三维点集合。例如，第i时刻的三维点集合可以称为{Vi}。
S150：获取Lidar点云数据，并在Lidar点云数据中识别时间戳与各关键帧的采集时间对应的关键Lidar点云数据，并根据Lidar测量模型，将关键Lidar点云数据投影至二维空间，并进行双线性插值，得到各Lidar深度图。
在一种实现方式中，可以通过时间戳信息找到时间上对应了以上关键帧的关键Lidar点云数据，同时Lidar的测量模型πl(·):R3→R2是已知的，此映射可以将三维空间的点云数据投影至二维空间上，再经过双线性插值，最终得到Lidar深度图，第i时刻的Lidar深度图可以称为{Ii}。
S160：根据各采集时刻对应的三维点集合与各Lidar深度图，构建关于由多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对优化函数进行优化，得到多相机模组到Lidar的外参。
在完成以上对相机图像和Lidar数据的各自预处理后，确定了每帧对应的视觉与Lidar数据：{Vi}与{Ii}，而要标定的参数是多相机模组到Lidar的外参
Figure BDA0002658138680000101
通过对{Vi}与{Ii}进行基于距离信息的关联，构建一个关于外参
Figure BDA0002658138680000102
的非线性最小二乘问题，即可优化得到多相机模组到Lidar的外参
Figure BDA0002658138680000103
在一种实现方式中，可以根据以下公式对多相机模组到Lidar的外参
Figure BDA0002658138680000104
进行优化：
Figure BDA0002658138680000105
πl(·)表示Lidar测量模型，Ii(x)表示i时刻对应的Lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，Vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
由上述内容可知，本发明实施例提供的用于自动驾驶车辆的Lidar标定方法，能够对多相机模组进行标定，得到各相机的内参以及各其他相机相对于主相机的外参后，基于标定得到的相机内参和外参，对各相机图像进行三维重建，得到各三维点集合，并且，可以将Lidar点云数据投影至二维空间得到Lidar深度图，从而可以根据三维点集合和Lidar深度图构建关于由多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，对函数进行优化得到多相机模组到Lidar的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。
在本发明实施例的一种实施方式中，如图2所示，对多相机模组进行标定的过程可以包括以下步骤：
S210：根据各相机图像的采集时刻，对各相机图像进行同步，得到各图像集；每个图像集中包含每个相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值。
在本发明实施例中，可以预先对各相机进行同步，并且可以设定各相机均具有相同的图像采集周期，以保证各相机均在相同的时刻进行图像采集。然而，在实际应用中，由于各相机时钟晶振等区别，各相机采集相机图像的时刻不完全相同。并且，在对相机进行标定时，只有对同一时刻各相机采集的相机图像进行对比，才能确定出各相机之间的位置关系，进而标定得到各相机的内参，和相机之间的外参。
因此，在获取到各相机图像后，可以对各相机图像进行同步，得到各图像集。也就是说，可以确定出属于同一时刻采集的相机图像，以准确的进行相机标定。
例如，当自动驾驶车辆中安装有相机A、B、C、D、E，分别获取到相机A采集的相机图像1、2、3、4、5、6；相机B采集的相机图像7、8、9、10、11、12；相机C采集的相机图像13、14、15、16、17、18；相机D采集的相机图像19、20、21、22、23、24；相机E采集的相机图像25、26、27、28、29、30时，对各相机图像进行同步后，可以分别得到以下图像集：
一、{1、7、13、19、25}
二、{2、8、14、20、26}
三、{3、9、15、21、27}
四、{4、10、16、22、28}
五、{5、11、17、23、29}
六、{6、12、18、24、30}。
S220：针对每张相机图像，提取该相机图像中的关键点，并根据所提取的关键点确定该相机图像的全局描述子。
例如，可以采用SIFT(Scale-invariant feature transform，尺度不变特征转换)算法来提取各相机图像中的关键点，采用bag-of-words的算法根据所提取的关键点确定各相机图像的全局描述子BoW。
S230：获取预先构建的特征地图，特征地图包括多帧图像，各图像包括各关键点和对应的描述子；针对每张相机图像，在特征地图中检索与该相机图像的全局描述子相似度最高的描述子对应的图像，作为该相机图像对应的匹配图像。
在本发明实施例中，为了提高相机标定的准确性，可以预先构建得到特征地图，以根据特征地图对相机进行标定。其中，上述特征地图可以为高精度地图，以保证相机标定的准确性。
也就是说，可以将特征地图作为参照物对相机进行标定，避免使用标定板等外在物体时，需要人工操作影响相机标定的准确性。其中，特征地图的关键点可以选用具有尺度不变性的图像特征点，如SIFT。特征地图中每帧图像的任一关键点均对应一局部描述子，各关键点的局部描述子构成该图像的全局描述子。
S240：将各相机图像和对应的匹配图像中的关键点进行匹配，并针对每张相机图像，随机采样该相机图像和对应的匹配图像中预设数量对关键点，根据采样得到的关键点计算该相机图像相对特征地图的位姿。
将各相机图像和对应的匹配图像中的关键点进行匹配，也即针对各相机图像中的关键点，在匹配图像中查找该关键点对应的三维点，并将匹配到的关键点建立关联关系。
进行关键点匹配后，即可根据相机图像和匹配图像计算各相机图像相对特征地图的位姿。例如，在本发明实施例中的各相机图像相对特征地图的位姿为4*4的矩阵时，可以针对每张相机图像，在该相机图像和对应的匹配图像中随机采样至少5对关键点，即可根据采样得到的关键点的坐标，对构建的该相机图像相对特征地图的位姿的方程进行求解，得到该相机图像相对特征地图的位姿。
S250：确定各相机中的主相机，针对每个其他相机，根据该相机所采集的各相机图像相对特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿。
在本发明实施例中，针对每个相机采集的每张相机图像，都可以计算得到该相机图像相对特征地图的位姿，然而，不同图像集中各相机图像相对特征地图的位姿的准确性不完全相同。
在一种实现方式中，可以将任一相机确定为主相机。确定主相机后，可以针对每个其他相机，根据该相机所采集的各相机图像相对特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿。
具体的，针对任一其他相机的任一相机图像相对特征地图的位姿，可以根据该位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各关键点对应的重投影误差，误差在预设范围内的关键点认为是内点。统计各相机图像的位姿对应的内点数，内点数最多的相机图像即为目标相机图像，该相机图像的位姿即为该相机的初始位姿。
S260：根据各图像集中主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对主相机的初始外参。
在一种实现方式中，针对每个图像集，可以确定该图像集对应的主相机坐标系为该图像集中主相机所采集相机图像的位姿。针对每个其他相机，可以确定该相机相对主相机的初始外参为，该相机对应的目标相机图像的初始位姿，乘以与该目标相机图像同图像集的主相机坐标系的逆。
例如，当任一时刻第i个相机所采集相机图像通过***获得的位姿是
Figure BDA0002658138680000141
第0个相机是主相机，则主相机的坐标系为：
Figure BDA0002658138680000142
第i个相机和主相机之间的初始外参就是
Figure BDA0002658138680000143
S270：根据各主相机坐标系，以及各其他相机相对主相机的初始外参，对预设优化函数进行优化，得到各相机的内参以及各其他相机相对于主相机的外参。
在一种实现方式中，可以根据以下公式(1)对各相机的内参进行优化：
Figure BDA0002658138680000144
Figure BDA0002658138680000145
表示j时刻世界系到相机i的坐标变换，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure BDA0002658138680000146
是由相机内参θi决定的相机的投影模型。
针对每个其他相机，可以根据以下公式(2)对该相机相对于主相机的外参进行优化：
Figure BDA0002658138680000147
Figure BDA0002658138680000148
表示j时刻主相机h的坐标系，也即世界系到主相机h的坐标变换，
Figure BDA0002658138680000149
表示相机i相对于主相机h的外参。
通过公式(1)进行优化时，所优化的是所有相机的内参和任一相机i在j时刻的位姿
Figure BDA00026581386800001410
通过公式(2)进行优化时，所优化的是各相机相对主相机的外参以及主相机在j时刻的位姿
Figure BDA00026581386800001411
在实际应用中，可以在自动驾驶车辆行驶过程中不断对各相机内参和各相机相对主相机的外参进行优化。具体的，自动驾驶车辆行驶过程中，各相机不断的进行相机图像采集，通过不断的根据各相机采集的相机图像对各相机内参，以及各相机相对主相机的外参进行优化。当每个相机和主相机同时被恢复位姿的对数达到阈值时，数据采集结束，根据所采集到的数据进行优化。
具体对相机内参和各相机相对主相机的外参进行优化时，针对任一相机，通过该相机的位姿和同时刻主相机的位姿可以计算得到该相机相对主相机的外参初值，通过主相机位姿乘以该相机的外参可以得到该相机的位姿，因此，上述两个公式可以相互转换。也就是说，公式(1)的优化结果可以传递给公式(2)进行优化，通过公式(2)进行优化后，还可以将优化结果传递给公式(1)进行再次优化，如此迭代循环，直到能量函数不下降或迭代次数超过阈值时，优化结束，将当前得到的优化结果作为最终的结果。
由上述内容可知，本实施例可以基于预先构建的特征地图确定各相机采集的相机图像的位姿，由于特征地图是固定不变的，各相机图像的位姿即可反映各相机之间的位置关系，因此能够基于各相机图像的位姿计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客观准确，从而能够提高相机标定的准确性。并且，基于特征地图对相机进行标定时，只需要预先构建得到特征地图即可，不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。
作为本发明实施例的另一种实施方式，对多相机模组进行标定的过程可以包括以下步骤：
S1：根据各相机图像的采集时刻，对各相机图像进行同步，得到各图像集；每个图像集中包含每个相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值。
S2：提取各相机图像中的特征点，并根据所提取的特征点对每两张相机图像进行特征匹配，得到各图像匹配对。
例如，可以采用SIFT(Scale-invariant feature transform，尺度不变特征转换)算法来提取各相机图像中的特征点。对每两张相机图像进行特征匹配时，可以针对每一个图像匹配对，计算对极几何，估计基础矩阵(Fundamental matrix)或者本质矩阵(Essential Matrix)，并通过随机抽样一致算法(Random Sample Consensus，RANSAC)优化改善匹配对。
S3：根据各图像匹配对进行场景重建，得到各相机图像的位姿，以及各相机图像中所包括特征点对应的世界系的3D点坐标。
例如，可以首先初始化选择两帧相机图像进行第一次光束平差法(BundleAdjustment，BA)，然后循环添加新的相机图像进行新的BA优化，最后直到所有的相机图像都被添加到BA优化中，此时就得到了所有相机图像的位姿和场景的视觉点云地图，也即各相机图像中所包括特征点对应的世界系的3D点坐标。
S4：确定各相机中的主相机，根据各图像集中主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；针对每个其他相机，根据各图像集中该相机所采集相机图像的位姿，以及各图像集对应的主相机坐标系，计算该相机在各主相机坐标系中的恢复位姿，并根据各恢复位姿计算该相机相对主相机的初始外参。
在一种实现方式中，可以将任一相机确定为主相机。确定主相机后，可以针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中主相机所采集相机图像的位姿；针对每个其他相机，确定该相机在任一主相机坐标系中的恢复位姿为，该主相机坐标系对应图像集中该相机所采集相机图像的位姿，与该主相机坐标系的逆的乘积。
例如，当任一图像集中第i个相机所采集相机图像通过***获得位姿是
Figure BDA0002658138680000161
第0个相机是主相机，则主相机的坐标系为：
Figure BDA0002658138680000162
第i个相机在该主相机坐标系中的恢复位姿
Figure BDA0002658138680000163
确定各图像集对应的主相机坐标系和各其他相机在主相机坐标系的恢复位姿后，可以针对每个其他相机，确定该相机相对主相机的初始外参为该相机在各主相机坐标系中的恢复位姿的平均值。通过多个图像集来计算各其他相机相对主相机的外参估计，能够提高计算结果的准确性。
S5：根据主相机的坐标系、各其他相机相对主相机的初始外参以及各特征点对应的世界系的3D点坐标，对预设优化函数进行优化，得到各相机的内参以及各其他相机相对于主相机的外参。
在一种实现方式中，可以根据以下公式对各相机的内参和各相机相对主相机的外参进行优化：
Figure BDA0002658138680000171
Figure BDA0002658138680000172
表示j时刻主相机h的坐标系，
Figure BDA0002658138680000173
表示相机i相对于主相机h的外参，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure BDA0002658138680000174
是由相机内参θi决定的相机的投影模型。
通过上述公式进行优化时，所优化的是所有相机的内参、各相机相对主相机的外参、主相机在j时刻的坐标系
Figure BDA0002658138680000175
以及各3D点坐标pw。
在实际应用中，可以在自动驾驶车辆行驶过程中不断对各相机内参和各相机相对主相机的外参进行优化。具体的，自动驾驶车辆行驶过程中，各相机不断的进行相机图像采集，通过不断的根据各相机采集的相机图像对各相机内参，以及各相机相对主相机的外参进行优化。当每个相机和主相机同时被恢复位姿的对数达到阈值时，数据采集结束，根据所采集到的数据进行优化。
由上述内容可知，本实施例可以根据各相机采集的相机图像进行场景重建，得到各相机图像的位姿以及各相机图像中所包括特征点对应的3D点坐标，场景重建结果即可反映各相机之间的位置关系，从而可以根据场景重建结果计算得到各相机的内参，以及各相机相对主相机的外参，整个标定方法不需要人工参与，不需要任何外在物体的辅助，结果客观准确，从而能够提高相机标定的准确性。并且，本实施例中的相机标定过程不需要建设标定场等固定的场地，从而可以减少资金投入，节省相机标定的成本。另外，与建设标定场来进行相机标定相比，本发明实施例的方案可以在自动驾驶车辆行驶至任意区域时进行相机标定，从而能够提高相机标定的便捷性。
作为本发明实施例的一种实施方式，对相机进行标定后，还可以基于标定得到的相机内参和各相机相对主相机的外参对轮速进行标定。
轮速标定的输入数据包括里程计输出的相对位姿以及轮速脉冲输出的帧间估计。其中，里程计输入为多相机图像，里程计得到各相机图像相对于第一帧相机图像的位姿，从而计算得到各帧之间的相对位姿。轮速编码器输出脉冲数，通过积分得到轮子的帧间位姿。
具体的，可以根据各相机图像，各相机的内参，以及各其他相机相对于主相机的外参，计算各相邻主相机图像之间的相对位姿；针对每帧主相机图像，确定从该主相机图像的采集时刻到上一帧主相机图像的采集时刻之间的时间为积分时间；获取轮速编码器输出的脉冲，根据输入的车轮半径依次对脉冲进行各积分时间的积分得到无人驾驶车辆的各帧间位姿；根据各相邻主相机图像之间的相对位姿，以及各帧间位姿，估计得到多个相机组成的多相机模组相对于轮速的外参初值；外参初值包括：俯仰角、滚转角、偏航角、尺度、横向平移以及纵向平移；对车轮半径进行优化，得到优化车轮半径；根据优化车轮半径依次对脉冲进行各积分时间的积分得到无人驾驶车辆到各帧间优化位姿；根据各相邻主相机图像之间的相对位姿，以及各帧间优化位姿，计算得到多相机模组相对于轮速的外参。
对轮速进行标定时，可以将整个多相机模组看作一个刚体，估计整个多相机模组在不同时刻的位姿。具体的，可以将各相机图像，各相机的内参，以及各其他相机相对于主相机的外参输入里程计，里程计得到各相机图像相对于第一帧相机图像的位姿，从而计算得到各相邻主相机图像之间的相对位姿。
确定积分时间时，例如，当任一帧主相机图像的采集时刻为2020.3.20.12:31，上一帧主相机图像的采集时刻为2020.3.20.12:33时，即可确定根据该主相机图像确定的积分时间为2020.3.20.12:31-2020.3.20.12:33。
对每帧主相机图像均进行上述计算后，即可得到n-1个积分时间，n为主相机图像的总数量。例如，当存在5帧主相机图像0、1、2、3、4时，得到的积分时间即为0-1、1-2、2-3、3-4，一共4个积分时间。计算帧间位姿时，输入的车轮半径可以为人工测量得到的出厂车轮半径。
在本发明实施例中，可以定义x轴方向为车横向，车体绕x轴的旋转为俯仰角pitch；y轴方向为车纵向，绕y轴的旋转为滚转角roll；z轴为高度，绕z轴的旋转为偏航角yaw。
在本发明实施例中，可以基于手眼标定算法对轮速进行标定。手眼标定算法是一种将两段轨迹对齐，并求解出二者转换关系的算法。在轮速标定中，这两段轨迹是相机轨迹和轮速轨迹，相机轨迹由里程计输出，为各时刻的位姿，轮速轨迹由脉冲积分获得，通过计算各帧位姿之间的差值，得到我们需要的相对位姿。
在一种实现方式中，可以在每个帧间位姿中识别出旋转分量，根据旋转分量通过欧拉角分解得到该帧间位姿对应的偏航角，并根据偏航角计算角速度；将角速度大于预设阈值的帧间位姿作为目标帧间位姿，以及与该帧间位姿对应时刻的相对位姿作为目标相对位姿；求解俯仰角和滚转角，并基于目标帧间位姿和目标相对位姿，通过奇异值分解SVD得到相机系到轮速系外参的初步结果
Figure BDA0002658138680000191
将各目标相对位姿左乘
Figure BDA0002658138680000192
作为更新后的目标相对位姿，再次进行SVD分解得到相机系到轮速系外参，直到外参结果变化值小于预设变化阈值时，得到偏航角、尺度、横向平移以及纵向平移。
例如，
Figure BDA0002658138680000193
为待估计的外参，即多相机模组相对于轮速的转换，
Figure BDA0002658138680000194
为轮子帧间位姿，
Figure BDA0002658138680000195
为里程计相对位姿，三者满足
Figure BDA0002658138680000196
通过两段轨迹，能够得到很多组
Figure BDA0002658138680000197
为了构建健康的求解问题，取出
Figure BDA0002658138680000198
的旋转部分，通过欧拉角分解得到这段时间轮子的偏航角yaw，除以时间间隔，得到角速度，角速度大的认为激励充足，用来求解外参，至此筛选出多组
Figure BDA0002658138680000199
Figure BDA00026581386800001910
为了保证标定效果，在标定过程中，要求载体要尽可能多转弯以保证足够的运动激励。由于轮子对高度的不可观测性，标定时载体要避免上下坡。
求解俯仰角和滚转角时，可以采用不同的算法计算。在一种实现方式中，可以获取在各主相机图像采集时刻范围内的激光雷达Lidar点云，并对Lidar点云进行平面拟合，得到Lidar系中的地平面法向量vl，获取Lidar系和相机系的外参，根据Lidar系和相机系的外参计算vl在相机系中的地平面法向量vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角。
在另一种实现方式中，可以将各主相机图像相对第一帧主相机图像的位姿进行平面拟合，得到在相机系中的地平面法向量计算vc，计算vc与轮速系中地平面法向量vo的角度，得到俯仰角和滚转角。
在另一种实现方式中，可以在各相邻主相机图像之间的相对位姿和各帧间位姿中，识别出旋转分量，基于识别出的旋转分量构建包含俯仰角和滚转角的关系式，并对关系式进行SVD分解得到俯仰角和滚转角。其中，上述关系式可以为：
Figure BDA00026581386800001911
在求解外参初值的过程中，误差有一部分来源于车轮半径，因此在进行线性求解后，会构建一个非线性优化问题，优化变量为多相机模组相对于轮速的外参以及车轮半径。
例如，可以通过以下公式，对车轮半径r进行优化：
Figure BDA0002658138680000201
Figure BDA0002658138680000202
表示多相机模组相对于轮速的外参，
Figure BDA0002658138680000203
表示
Figure BDA0002658138680000204
的旋转分量，
Figure BDA0002658138680000205
表示
Figure BDA0002658138680000206
的平移分量，
Figure BDA0002658138680000207
表示各帧间位姿矩阵的平移分量，
Figure BDA0002658138680000208
表示各相对位姿矩阵的平移分量，s表示尺度，I表示单位矩阵。
对车轮半径进行优化后，重新对轮速脉冲积分，重复上述步骤即可计算得到多相机模组相对于轮速的外参。
本实施例中，对多相机模组进行标定后，还可以基于标定得到的相机内参和外参，对轮速进行标定，得到多相机模组相对于轮速的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。在帧间位姿中，角速度较大的表明激励充足，在轮速标定过程中，选取角速度较大的帧间位姿，以及与所选取的帧间位姿对应时刻的相对位姿来计算多相机模组相对于轮速的外参，能够提高外参标定的准确性。在进行轮速标定时，人工测量的车轮半径会有误差，从而影响帧间位姿的准确性，因此，在得到外参初值后对车轮半径进行优化，并根据优化后的车轮半径计算得到多相机模组相对于轮速的外参，能够提高外参计算的准确性。
作为本发明实施例的一种实施方式，对相机、Lidar和轮速均完成标定后，还可以将计算得到的各相机相对于主相机的外参，以及多相机模组到Lidar的外参，转换至轮速系中。
具体的，可以针对每个其他相机，将该相机相对于主相机的外参，与多相机模组相对于轮速的外参的逆矩阵的乘积，确定为该相机在轮速系中的外参。可以将多相机模组到Lidar的外参，与多相机模组相对于轮速的外参的逆矩阵的乘积，确定为Lidar在轮速系中的外参。
对相机进行标定时，计算得到的相机外参均以主相机为参考系，而在无人驾驶车辆***中，我们更需要以车体系，也就是轮速系为参照系，因此，可以将各相机在主相机参考系中的外参，转换至轮速系中，可以在后续对各传感器采集的数据进行处理时，提高数据处理的准确性。
对Lidar进行标定时，计算得到的外参为以主相机为参考系，而在无人驾驶车辆***中，我们更需要以车体系，也就是轮速系为参照系，因此，可以将Lidar在主相机参考系中的外参，转换至轮速系中，可以在后续对各传感器采集的数据进行处理时，提高数据处理的准确性。
如图3所示，其示出了本发明实施例提供一种用于自动驾驶车辆的Lidar标定装置的结构示意图，所述自动驾驶车辆安装有多个相机，所述装置包括：
相机标定模块310，用于获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参；所述主相机为所述各相机中任一相机；
目标位姿确定模块320，用于根据各所述相机的内参以及所述各其他相机相对于所述主相机的外参，对各所述相机图像进行特征点的检测与匹配，得到各所述相机图像的目标位姿；
关键帧确定模块330，用于针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧；
深度估计模块340，用于针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息；并根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合；
数据投影模块350，用于获取Lidar点云数据，在所述Lidar点云数据中识别时间戳与各所述关键帧的采集时间对应的关键Lidar点云数据；并根据Lidar测量模型，将所述关键Lidar点云数据投影至二维空间，并进行双线性插值，得到各Lidar深度图；
外参标定模块360，用于根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参。
可选的，所述外参标定模块360，具体用于：
根据以下公式对所述多相机模组到Lidar的外参
Figure BDA0002658138680000221
进行优化：
Figure BDA0002658138680000222
πl(·)表示所述Lidar测量模型，Ii(x)表示i时刻对应的Lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，Vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
可选的，所述相机标定模块包括：
图像同步子模块，用于根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
描述子确定子模块，用于针对每张所述相机图像，提取该相机图像中的关键点，并根据所提取的关键点确定该相机图像的全局描述子；
图像匹配子模块，用于获取预先构建的特征地图，所述特征地图包括多帧图像，各所述图像包括各关键点和对应的描述子；针对每张相机图像，在所述特征地图中检索与该相机图像的全局描述子相似度最高的描述子对应的图像，作为该相机图像对应的匹配图像；
位姿计算子模块，用于将各所述相机图像和对应的匹配图像中的关键点进行匹配，并针对每张相机图像，随机采样该相机图像和对应的匹配图像中预设数量对关键点，根据采样得到的关键点计算该相机图像相对所述特征地图的位姿；
初始位姿确定子模块，用于确定各所述相机中的主相机，针对每个其他相机，根据该相机所采集的各相机图像相对所述特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿；
初始外参计算子模块，用于根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参；
相机标定子模块，用于根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
可选的，所述初始外参计算子模块，具体用于：
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
针对每个其他相机，确定该相机相对所述主相机的初始外参为，该相机对应的目标相机图像的初始位姿，乘以与该目标相机图像同图像集的主相机坐标系的逆。
可选的，所述相机标定子模块，具体用于：
根据以下公式对各相机的内参进行优化：
Figure BDA0002658138680000231
Figure BDA0002658138680000232
表示j时刻世界系到相机i的坐标变换，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure BDA0002658138680000233
是由相机内参θi决定的相机的投影模型；
针对每个所述其他相机，根据以下公式对该相机相对于所述主相机的外参进行优化：
Figure BDA0002658138680000234
Figure BDA0002658138680000235
表示j时刻主相机h的坐标系，
Figure BDA0002658138680000236
表示相机i相对于主相机h的外参。
由上述内容可知，本发明实施例提供的用于自动驾驶车辆的Lidar标定装置，能够对多相机模组进行标定，得到各相机的内参以及各其他相机相对于主相机的外参后，基于标定得到的相机内参和外参，对各相机图像进行三维重建，得到各三维点集合，并且，可以将Lidar点云数据投影至二维空间得到Lidar深度图，从而可以根据三维点集合和Lidar深度图构建关于由多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，对函数进行优化得到多相机模组到Lidar的外参，从而在自动驾驶车辆行驶过程中，可以对各传感器采集的数据进行准确的处理，进而保证自动驾驶车辆行驶的安全性。
上述装置实施例与方法实施例相对应，与该方法实施例具有同样的技术效果，具体说明参见方法实施例。装置实施例是基于方法实施例得到的，具体的说明可以参见方法实施例部分，此处不再赘述。
本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。
本领域普通技术人员可以理解：实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。

Claims (10)

1.一种用于自动驾驶车辆的Lidar标定方法，其特征在于，所述自动驾驶车辆安装有多个相机，所述方法包括：
获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参；所述主相机为所述各相机中任一相机；
根据各所述相机的内参以及所述各其他相机相对于所述主相机的外参，对各所述相机图像进行特征点的检测与匹配，得到各所述相机图像的目标位姿；
针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧；
针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息；并根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合；
获取Lidar点云数据，在所述Lidar点云数据中识别时间戳与各所述关键帧的采集时间对应的关键Lidar点云数据；并根据Lidar测量模型，将所述关键Lidar点云数据投影至二维空间，并进行双线性插值，得到各Lidar深度图；
根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参。
2.根据权利要求1所述的方法，其特征在于，所述根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参的步骤包括：
根据以下公式对所述多相机模组到Lidar的外参
Figure FDA0002658138670000011
进行优化：
Figure FDA0002658138670000021
πl(·)表示所述Lidar测量模型，Ii(x)表示i时刻对应的Lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，Vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
3.根据权利要求1或2所述的方法，其特征在于，所述根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参的步骤包括：
根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
针对每张所述相机图像，提取该相机图像中的关键点，并根据所提取的关键点确定该相机图像的全局描述子；
获取预先构建的特征地图，所述特征地图包括多帧图像，各所述图像包括各关键点和对应的描述子；针对每张相机图像，在所述特征地图中检索与该相机图像的全局描述子相似度最高的描述子对应的图像，作为该相机图像对应的匹配图像；
将各所述相机图像和对应的匹配图像中的关键点进行匹配，并针对每张相机图像，随机采样该相机图像和对应的匹配图像中预设数量对关键点，根据采样得到的关键点计算该相机图像相对所述特征地图的位姿；
确定各所述相机中的主相机，针对每个其他相机，根据该相机所采集的各相机图像相对所述特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿；
根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参；
根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
4.根据权利要求3所述的方法，其特征在于，所述根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参的步骤包括：
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
针对每个其他相机，确定该相机相对所述主相机的初始外参为，该相机对应的目标相机图像的初始位姿，乘以与该目标相机图像同图像集的主相机坐标系的逆。
5.根据权利要求3或4所述的方法，其特征在于，所述根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参的步骤包括：
根据以下公式对各相机的内参进行优化：
Figure FDA0002658138670000031
Figure FDA0002658138670000032
表示j时刻世界系到相机i的坐标变换，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure FDA0002658138670000036
是由相机内参θi决定的相机的投影模型；
针对每个所述其他相机，根据以下公式对该相机相对于所述主相机的外参进行优化：
Figure FDA0002658138670000033
Figure FDA0002658138670000034
表示j时刻主相机h的坐标系，
Figure FDA0002658138670000035
表示相机i相对于主相机h的外参。
6.一种用于自动驾驶车辆的Lidar标定装置，其特征在于，所述自动驾驶车辆安装有多个相机，所述装置包括：
相机标定模块，用于获取各所述相机采集的相机图像，以及各所述相机图像对应的采集时刻；根据各所述相机图像计算各所述相机的内参以及各其他相机相对于主相机的外参；所述主相机为所述各相机中任一相机；
目标位姿确定模块，用于根据各所述相机的内参以及所述各其他相机相对于所述主相机的外参，对各所述相机图像进行特征点的检测与匹配，得到各所述相机图像的目标位姿；
关键帧确定模块，用于针对每个相机采集的目标相机图像，根据各目标相机图像的采集时刻和目标位姿，确定各目标相机图像与前一帧目标相机图像的距离和角度间隔，并在任一目标相机图像与前一帧目标相机图像的距离大于预设距离阈值，且角度间隔大于预设角度阈值时，将各相机在该目标相机图像采集时刻所采集的相机图像均确定为关键帧；
深度估计模块，用于针对各关键帧，确定该关键帧中梯度值大于预设梯度阈值的梯度点，并根据该关键帧的目标位姿对该关键帧中各梯度点进行深度估计得到该关键帧对应的局部三维点信息；并根据各关键帧的采集时刻，以及各关键帧对应的局部三维点信息，统计得到各采集时刻对应的三维点集合；
数据投影模块，用于获取Lidar点云数据，在所述Lidar点云数据中识别时间戳与各所述关键帧的采集时间对应的关键Lidar点云数据；并根据Lidar测量模型，将所述关键Lidar点云数据投影至二维空间，并进行双线性插值，得到各Lidar深度图；
外参标定模块，用于根据所述各采集时刻对应的三维点集合与所述各Lidar深度图，构建关于由所述多个相机组成的多相机模组到Lidar的外参的非线性最小二乘优化函数，并对所述优化函数进行优化，得到所述多相机模组到Lidar的外参。
7.根据权利要求6所述的装置，其特征在于，所述外参标定模块，具体用于：
根据以下公式对所述多相机模组到Lidar的外参
Figure FDA0002658138670000041
进行优化：
Figure FDA0002658138670000051
πl(·)表示所述Lidar测量模型，Ii(x)表示i时刻对应的Lidar深度图在二维坐标x(u,v)的取值，pc表示相机系下的三维点坐标，Vi表示i时刻对应的三维点集合，‖pc‖表示三维点pc(x,y,z)的模长。
8.根据权利要求6或7所述的装置，其特征在于，所述相机标定模块包括：
图像同步子模块，用于根据各所述相机图像的采集时刻，对各所述相机图像进行同步，得到各图像集；每个图像集中包含每个所述相机所采集的一张相机图像，每个图像集中任两张相机图像的采集时刻之差小于预设时间阈值；
描述子确定子模块，用于针对每张所述相机图像，提取该相机图像中的关键点，并根据所提取的关键点确定该相机图像的全局描述子；
图像匹配子模块，用于获取预先构建的特征地图，所述特征地图包括多帧图像，各所述图像包括各关键点和对应的描述子；针对每张相机图像，在所述特征地图中检索与该相机图像的全局描述子相似度最高的描述子对应的图像，作为该相机图像对应的匹配图像；
位姿计算子模块，用于将各所述相机图像和对应的匹配图像中的关键点进行匹配，并针对每张相机图像，随机采样该相机图像和对应的匹配图像中预设数量对关键点，根据采样得到的关键点计算该相机图像相对所述特征地图的位姿；
初始位姿确定子模块，用于确定各所述相机中的主相机，针对每个其他相机，根据该相机所采集的各相机图像相对所述特征地图的位姿，以及该相机所采集的各相机图像和对应的匹配图像中的关键点坐标，计算各位姿对应的内点数，并将内点数最高的位姿确定为该相机对应的目标相机图像的初始位姿；
初始外参计算子模块，用于根据各图像集中所述主相机所采集相机图像的位姿，计算各图像集对应的主相机坐标系；并针对每个所述其他相机，根据该相机对应的目标相机图像的初始位姿，以及与该目标相机图像同图像集的主相机坐标系，计算该相机相对所述主相机的初始外参；
相机标定子模块，用于根据各所述主相机坐标系，以及各所述其他相机相对所述主相机的初始外参，对预设优化函数进行优化，得到各所述相机的内参以及各所述其他相机相对于所述主相机的外参。
9.根据权利要求8所述的装置，其特征在于，所述初始外参计算子模块，具体用于：
针对每个图像集，确定该图像集对应的主相机坐标系为该图像集中所述主相机所采集相机图像的位姿；
针对每个其他相机，确定该相机相对所述主相机的初始外参为，该相机对应的目标相机图像的初始位姿，乘以与该目标相机图像同图像集的主相机坐标系的逆。
10.根据权利要求8或9所述的装置，其特征在于，所述相机标定子模块，具体用于：
根据以下公式对各相机的内参进行优化：
Figure FDA0002658138670000061
Figure FDA0002658138670000062
表示j时刻世界系到相机i的坐标变换，θi表示相机i的内参，I表示所有相机集合，J表示所有采集时刻集合，pw为世界系的3D点坐标，u为与3D点匹配的相机图像中的像素点坐标，
Figure FDA0002658138670000063
是由相机内参θi决定的相机的投影模型；
针对每个所述其他相机，根据以下公式对该相机相对于所述主相机的外参进行优化：
Figure FDA0002658138670000064
Figure FDA0002658138670000065
表示j时刻主相机h的坐标系，
Figure FDA0002658138670000066
表示相机i相对于主相机h的外参。
CN202010894839.XA 2020-08-31 2020-08-31 一种用于自动驾驶车辆的Lidar标定方法及装置 Pending CN114119763A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010894839.XA CN114119763A (zh) 2020-08-31 2020-08-31 一种用于自动驾驶车辆的Lidar标定方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010894839.XA CN114119763A (zh) 2020-08-31 2020-08-31 一种用于自动驾驶车辆的Lidar标定方法及装置

Publications (1)

Publication Number Publication Date
CN114119763A true CN114119763A (zh) 2022-03-01

Family

ID=80359699

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010894839.XA Pending CN114119763A (zh) 2020-08-31 2020-08-31 一种用于自动驾驶车辆的Lidar标定方法及装置

Country Status (1)

Country Link
CN (1) CN114119763A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114758005A (zh) * 2022-03-23 2022-07-15 中国科学院自动化研究所 激光雷达与相机外参标定方法及装置
CN115100281A (zh) * 2022-06-20 2022-09-23 江苏集萃未来城市应用技术研究所有限公司 基于三维标定板的激光雷达与相机的外内参标定方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114758005A (zh) * 2022-03-23 2022-07-15 中国科学院自动化研究所 激光雷达与相机外参标定方法及装置
CN115100281A (zh) * 2022-06-20 2022-09-23 江苏集萃未来城市应用技术研究所有限公司 基于三维标定板的激光雷达与相机的外内参标定方法

Similar Documents

Publication Publication Date Title
CN111436216B (zh) 用于彩色点云生成的方法和***
CN102999759B (zh) 一种基于光流的车辆运动状态估计方法
CN103559711B (zh) 基于三维视觉***图像特征和三维信息的运动估计方法
CN108335337B (zh) 一种正射影像图的生成方法及装置
CN108733039A (zh) 一种机器人室内导航定位的方法与装置
CN103954283A (zh) 基于景象匹配/视觉里程的惯性组合导航方法
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
US20180075614A1 (en) Method of Depth Estimation Using a Camera and Inertial Sensor
CN108917753B (zh) 基于从运动恢复结构的飞行器位置确定方法
CN115690338A (zh) 地图构建方法、装置、设备及存储介质
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN112837352A (zh) 基于图像的数据处理方法、装置及设备、汽车、存储介质
CN114119763A (zh) 一种用于自动驾驶车辆的Lidar标定方法及装置
US20210225012A1 (en) System and method for egomotion estimation
CN114219852A (zh) 一种用于自动驾驶车辆的多传感器标定方法及装置
CN113313659A (zh) 一种多机协同约束下高精度图像拼接方法
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN111583342A (zh) 一种基于双目视觉的目标快速定位方法及装置
CN113643355B (zh) 一种目标车辆位置和朝向的检测方法、***及存储介质
CN106408589A (zh) 基于车载俯视相机的车辆运动测量方法
CN117333846A (zh) 恶劣天气下基于传感器融合和增量学习的检测方法及***
CN116736322A (zh) 融合相机图像与机载激光雷达点云数据的速度预测方法
CN114638858B (zh) 一种基于车载双相机***的运动目标位置与速度估计方法
CN116151320A (zh) 一种抗动态目标干扰的视觉里程计方法及视觉里程计装置
CN114202577A (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
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20220308

Address after: 100083 unit 501, block AB, Dongsheng building, No. 8, Zhongguancun East Road, Haidian District, Beijing

Applicant after: BEIJING MOMENTA TECHNOLOGY Co.,Ltd.

Address before: 100083 room 28, 4 / F, block a, Dongsheng building, 8 Zhongguancun East Road, Haidian District, Beijing

Applicant before: BEIJING CHUSUDU TECHNOLOGY Co.,Ltd.