[发明专利]一种基于BA图优化的可见光定位与深度相机紧耦合方法

权利要求书

1.一种基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，包括以下步骤：

S1、对LED灯光进行调制；

S2、通过工业相机捕获调制后的所述LED灯光的信息，并对所述信息进行解算，获得工业相机位资信息；

S3、对所述工业相机位资信息进行坐标变换处理，获得深度相机Pose类型数据信息，并基于所述深度相机Pose类型数据信息估算深度相机位姿信息与地图点信息；

S4、将所述深度相机Pose类型数据信息、所述深度相机位姿与地图点信息紧耦合到BA优化函数中进行优化，所述优化结果用于SLAM建图。

2.根据权利要求1所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述S1中调制的具体方法为：使所述LED灯光按照不同频率的PWM波进行闪烁，其中，不同频率的所述PWM波对应不同调制信号，并对所述不同调制信号分配不同ID信息，完成灯光调制。

3.根据权利要求2所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述S2中捕获的具体方法为：基于所述不同ID信息获取所述LED灯光的位置信息。

4.根据权利要求1所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述深度相机Pose类型数据信息包括工业相机在世界坐标系下的绝对坐标三维信息以及工业相机的四元数信息。

5.根据权利要求1所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述S3中深度相机Pose类型数据信息的获取方法为：

S3.1、对t时刻的所述工业相机位资信息进行提取，获得符合优化要求的工业相机Pose类型数据信息；

S3.2、获取所述工业相机和所述深度相机的相对位置信息，根据所述相对位置信息对所述工业相机Pose类型数据信息进行坐标变换处理，得到深度相机Pose类型数据信息。

6.根据权利要求1所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述S3中的估算方法采用ORBSLAM2估算方法。

7.根据权利要求1所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述S4中紧耦合的方法为：

S4.1、将t时刻的所述深度相机Pose类型数据信息和所述深度相机位姿信息与地图点信息进行局部BA图优化，并构建g2o优化器；

S4.2、基于所述g2o优化器，将t时刻的所述深度相机Pose类型数据信息、所述深度相机位姿信息与地图点信息作为BA图优化过程中的三个顶点，并构建误差函数边，完成紧耦合。

8.根据权利要求7所述的基于BA图优化的可见光定位与深度相机紧耦合方法，其特征在于，所述误差函数边的构建方法包括：

S4.2.1、将t时刻的所述深度相机Pose类型数据信息和所述深度相机位姿信息进行求差计算，获取误差数据，并基于所述误差数据获取t时刻的所述深度相机Pose类型数据信息和所述深度相机位姿信息之间的误差函数边；

S4.2.2、对t时刻的所述深度相机Pose类型数据信息和所述深度相机地图点信息进行重投影误差计算，获得t时刻的所述深度相机Pose类型数据信息和所述深度相机地图点信息之间的误差函数边。