[发明专利]基于特征点线的单目相机位姿估计和优化方法及系统

摘要

本发明公开了一种基于特征点线的单目相机位姿估计和优化方法及系统，包括步骤S1基于点约束和线约束构造超定线性系统，并建立旋转矩阵和平移向量的关系；根据点约束和线约束的子约束的代价函数，利用最小二乘法构建总代价函数，通过引入凯里参数表达，获得最终的代价函数，利用该最终的代价函数求解相机位姿的旋转矩阵和平移向量；S2在点约束和线约束中引入桑普森误差构建目标函数，以步骤S1所得旋转矩阵和平移向量为初始值，采用FNS迭代器优化旋转矩阵和平移向量。本发明对单目相片获取设备和相片场景均无限制，适用于车载单目相机、机载单目相机和手持式单目相机的相片；也适用于道路场景、航空场景和室内场景的相片。

主权项

基于特征点线的单目相机位姿估计和优化方法，其特征是，包括步骤：S1基于m个特征点和n条特征线的几何约束构建代价函数，利用代价函数求解相机位姿的旋转矩阵R和平移向量t，其中，m+n≥3；本步骤进一步包括：S101基于几何约束分别建立各特征点矩阵形式的点约束和各特征线矩阵形式的线约束；S102联立所有的点约束和线约束构造超定线性系统；S103求解超定线性系统，获得t关于R的表达式；S104根据最小二乘法构建总代价函数F(R,t)：其中：fi(R,t)表示第i个点约束的子代价函数，gj(R,t)表示第j个线约束的子代价函数，pgci,1和pgci,2为第i个点约束中两个相互独立的子约束；lgci,1和lgci,2为第j个线约束中两个相互独立的子约束；S105引入凯里参数化描述R，使F(R,t)转化为四阶多项式，得到最终的代价函数F(s)；S106令F(s)分别对各特征点和各特征线对应R的分解参数矩阵的一阶偏导为0，计算驻点；S107应用基技术自动恢复旋转矩阵R和平移向量t；S2对步骤S1获得的旋转矩阵R和平移向量t进行优化；本步骤进一步包括：S201将所有的点约束和线约束表示为形式，ω＝m+n，其表示条件数；ψω由观察值ηω构成，ηω为特征线两端点组成的2*2二维坐标矩阵；x包含来自R和t的所有元素；S202对引入桑普森误差构建目标函数，以步骤S1获得的R和t为初始值，利用FNS迭代器迭求解x，从而获得优化后的R和t；S203对优化后的R进行奇异值分解以微调R；S204根据微调后的R和步骤S103所获得的t关于R的表达式，计算优化后的t。