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

权利要求书

1.基于特征点线的单目相机位姿估计和优化方法，其特征是，包括步骤：

S1基于m个特征点和n条特征线的几何约束构建代价函数，利用代价函数求解相机位姿的旋转矩阵R和平移向量t，其中，m+n≥3；本步骤进一步包括：

S101基于几何约束分别建立各特征点矩阵形式的点约束和各特征线矩阵形式的线约束；

S102联立所有的点约束和线约束构造超定线性系统；

S103求解超定线性系统，获得t关于R的表达式；

S104根据最小二乘法构建总代价函数F(R,t)：其中：f_i(R,t)表示第i个点约束的子代价函数，g_j(R,t)表示第j个线约束的子代价函数，pgc_i,1和pgc_i,2为第i个点约束中两个相互独立的子约束；lgc_i,1和lgc_i,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。

2.如权利要求1所述的基于特征点线的单目相机位姿估计和优化方法，其特征是：

所述各特征点矩阵形式的几何约束{PGC_i}为其中，{PGC_i}表示第i个三维特征点的几何约束；表示第i个三维特征点在相片平面上所对应的二维特征点的v坐标；p_i表示第i个三维特征点的三维坐标；t表示从世界坐标系向相机坐标系的平移向量。

3.如权利要求1所述的基于特征点线的单目相机位姿估计和优化方法，其特征是：

所述各特征线矩阵形式的几何约束{LGC_j}为其中，{LGC_j}表示第j条三维特征线的几何约束；表示第j条三维特征线的三维方向向量的第一维元素；表示第j条三维特征线；K表示相机的内参矩阵；表示第j条三维特征线与原点构成的平面的三维法向量的第三维元素；表示第j条三维特征线与原点构成的平面的三维法向量的第二维元素；T表示对矩阵进行转置；t和R分别表示从世界坐标系向相机坐标系的平移向量和旋转矩阵。

4.如权利要求1所述的基于特征点线的单目相机位姿估计和优化方法，其特征是：

进一步的，子步骤S105具体为：

使用凯里参数化描述旋转矩阵

根据t关于R的表达式，将t表示为的形式；

去掉R和t的表达式中的系数，将R和t的表达式代入总代价函数F(R,t)中，得到最终的代价函数F(s)，记为

其中，s＝1+b²+c²+d²；b，c，d均为旋转矩阵R的分解参数；U是系数矩阵；

ρ＝[b²,bc,bd,b,c²,cd,d²,d,1]^T；h(s)为具有未知可变向量s＝[b,c,d]^T的二次多项式。

5.如权利要求1所述的基于特征点线的单目相机位姿估计和优化方法，其特征是：

步骤S202中，所述目标函数为其中，H(x)表示桑普森误差；为描述ψ_ω不确定性的协方差矩阵。

6.基于特征点线的单目相机位姿估计和优化系统，其特征是，包括：

第一模块，用来基于m个特征点和n条特征线的几何约束构建代价函数，利用代价函数求解相机位姿的旋转矩阵R和平移向量t，其中，m+n≥3；

所述第一模块进一步包括子模块：

第一子模块，用来基于几何约束分别建立各特征点矩阵形式的点约束和各特征线矩阵形式的线约束；

第二子模块，用来联立所有的点约束和线约束构造超定线性系统；

第三子模块，用来求解超定线性系统，获得t关于R的表达式；

第四子模块，用来根据最小二乘法构建总代价函数F(R,t)：

minR,tF(R,t)=ΔΣi=0mfi2(R,t)+Σj=0ngj2(R,t);]]>

其中：f_i(R,t)表示第i个点约束的子代价函数，g_j(R,t)表示第j个线约束的子代价函数，pgc_i,1和pgc_i,2为第i个点约束中两个相互独立的子约束；lgc_i,1和lgc_i,2为第j个线约束中两个相互独立的子约束；

第五子模块，用来引入凯里参数化描述R，使F(R,t)转化为四阶多项式，得到最终的代价函数F(s)；

第六子模块，用来令F(s)分别对各特征点和各特征线对应R的分解参数矩阵的一阶偏导为0，计算驻点；

第七子模块，用来应用基技术自动恢复旋转矩阵R和平移向量t；

第二模块，用来对第一模块获得的旋转矩阵R和平移向量t进行优化；

所述第二模块进一步包括：

第八子模块，用来将所有的点约束和线约束表示为形式，ω＝m+n，其表示条件数；ψ_ω由观察值η_ω构成，η_ω为特征线两端点组成的2*2二维坐标矩阵；x包含来自R和t的所有元素；

第九子模块，用来对引入桑普森误差构建目标函数，以第一模块获得的R和t为初始值，利用FNS迭代器迭求解x，从而获得优化后的R和t；

第十子模块，用来对优化后的R进行奇异值分解以微调R；

第十一子模块，用来根据微调后的R和第三子模块所获得的t关于R的表达式，计算优化后的t。