本发明属于即时定位与地图构建技术领域，特别是涉及一种基于特征点线的单目相机位姿估计和优化方法及系统。
背景技术：
视觉测量是即时定位与地图构建技术中的重要研究课题之一。近年来，视觉测量已经被充分研究并广泛应用于机器人，自主驾驶，可穿戴计算和增强现实，并且在一些例如全球定位系统无效的室内和水下环境的特别情况下尤其适用。其中单目视觉测量使用单目摄像机的视觉光度仪具有更广阔的应用前景，更便宜，更紧凑，更容易校准等的优点。单目视觉测量的最标准程序是首先提取一组图像特征并在连续帧中匹配它们，然后重建增量3d结构和最佳相机位姿，最后通过重投影误差最小化改善结构和位姿。因此，研究相机位姿估计和优化，对即时定位与地图构建成果有着重要意义。
一般来说，现有的相机位姿估计算法可以基于所使用特征的类型被分为三类：基于点的相机位姿估计，基于线的相机位姿估计，以及点线组合的相机位姿估计。
基于点的相机位姿估计依赖于三维点到二维点的对应，由于点特征可以被容易地检测到，并且在许多环境中具有良好的效率和性能，因此可利用这一特征建立三维空间中的点与二维相片中的点的对应并以此来估计相机的位姿。该方法存在的问题是当系统包含大量观测值时，这些方法就无法应用或变得十分低效，而且在低纹理场景特别是人造环境中效果很差。
基于线的相机位姿估计应用合适的方法检测线段，再进行匹配以估计相机的位姿。该方法存在的问题是精度不高，需要加入其它约束和优化以获得更精确的结果。而且方法步骤复杂计算效率也很低，耗时较长。
点线组合的相机位姿估计目前还没有得到很好的解决，相关的工作也非常有限，大部分方法为将线性约束与基于点的算法集成。这些方法的缺点是当特征稀缺时它们将是无效的。即使使用共线性和共面约束来解决极端条件，这种方法也是针对不同情况使用不同的模型，无法视点和线为等位条件，做不到真正的点线结合，而且难以扩展用于冗余观察。
技术实现要素：
本发明的目的是提供一种做到真正点线结合的、基于特征点线的单目相机位姿估计和优化方法及系统。
本发明基于特征点线的单目相机位姿估计和优化方法，包括步骤：
s1基于m个特征点和n条特征线的几何约束构建代价函数，利用代价函数求解相机位姿的旋转矩阵r和平移向量t，其中，m+n≥3；本步骤进一步包括：
s101基于几何约束分别建立各特征点矩阵形式的点约束和各特征线矩阵形式的线约束；
s102联立所有的点约束和线约束构造超定线性系统；
s103求解超定线性系统，获得t关于r的表达式；
s104根据最小二乘法构建总代价函数f(r,t)：
s105引入凯里参数化描述r，使f(r,t)转化为四阶多项式，得到最终的代价函数f(s)；
s106令f(s)分别对各特征点和各特征线对应r的分解参数矩阵的一阶偏导为0，计算驻点；
s107应用
s2对步骤s1获得的旋转矩阵r和平移向量t进行优化；本步骤进一步包括：
s201将所有的点约束和线约束表示为
s202对
s203对优化后的r进行奇异值分解以微调r；
s204根据微调后的r和步骤s103所获得的t关于r的表达式，计算优化后的t。
进一步的，所述各特征点矩阵形式的几何约束{pgci}为
进一步的，所述各特征线矩阵形式的几何约束{lgcj}为
进一步的，子步骤s105具体为：
使用凯里参数化描述旋转矩阵
根据t关于r的表达式，将t表示为
去掉r和t的表达式中的系数，将r和t的表达式代入总代价函数f(r,t)中，得到最终的代价函数f(s)，记为
其中，s＝1+b2+c2+d2；b，c，d均为旋转矩阵r的分解参数；u是系数矩阵；ρ＝[b2,bc,bd,b,c2,cd,d2,d,1]t；h(s)为具有未知可变向量s＝[b,c,d]t的二次多项式。
进一步的，步骤s202中，所述目标函数为
本发明基于特征点线的单目相机位姿估计和优化系统，包括：
第一模块，用来基于m个特征点和n条特征线的几何约束构建代价函数，利用代价函数求解相机位姿的旋转矩阵r和平移向量t，其中，m+n≥3；
所述第一模块进一步包括子模块：
第一子模块，用来基于几何约束分别建立各特征点矩阵形式的点约束和各特征线矩阵形式的线约束；
第二子模块，用来联立所有的点约束和线约束构造超定线性系统；
第三子模块，用来求解超定线性系统，获得t关于r的表达式；
第四子模块，用来根据最小二乘法构建总代价函数f(r,t)：
其中：fi(r,t)表示第i个点约束的子代价函数，
第五子模块，用来引入凯里参数化描述r，使f(r,t)转化为四阶多项式，得到最终的代价函数f(s)；
第六子模块，用来令f(s)分别对各特征点和各特征线对应r的分解参数矩阵的一阶偏导为0，计算驻点；
第七子模块，用来应用
第二模块，用来对第一模块获得的旋转矩阵r和平移向量t进行优化；
所述第二模块进一步包括：
第八子模块，用来将所有的点约束和线约束表示为
第九子模块，用来对
第十子模块，用来对优化后的r进行奇异值分解以微调r；
第十一子模块，用来根据微调后的r和第三子模块所获得的t关于r的表达式，计算优化后的t。
本发明具有如下优点和有益效果：
本发明通过鲁棒的几何约束和可靠的代数求解器，直接检索在不进行初始化或迭代的情况下通过一阶最优性条件最小化的代价函数的所有固定点，得到可靠的相机位姿初始值。并且为了改进相机位姿，本发明提出了一种新的优化策略，通过考虑每个特征的特定不确定性来最小化无约束sampson误差，以更合理地减小噪声影响。此外，本发明还通过避免高维参数搜索，使过程比传统的光束平差法更简单。本发明提的第一模型可以等效地处理点和线要素，这适用于所有需要最小数量为3的点和线要素的所有最小情况，并且具有可以容易地扩展到具有更多冗余观测值的各种情况的优点。
本发明方法对单目相片获取设备无限制，适用于车载单目相机、机载单目相机和手持式单目相机的相片；对相片场景也无限制，适用于道路场景、航空场景和室内场景的相片。
附图说明
图1为本发明方法的具体流程示意图；
图2为本发明几何约束的原理示意图。
具体实施方式
下面将结合附图和具体实施方式，进一步说明本发明技术方案。
见图1，本发明基于特征点和特征线的单目相机位姿估计和优化方法，具体步骤如下：
步骤1，建立特征点和特征线的几何约束，并依此构建代价函数，利用代价函数求解相机位姿初始的旋转矩阵和平移向量。
本步骤包括以下子步骤：
步骤101，建立特征点和特征线矩阵形式的几何约束，即点约束和线约束。
由相片与空间之间的三点共线及相交线共面的几何约束，可构造出所需的点约束和线约束。本发明所采用几何约束的原理见图2，图中描述了等价使用特征点和特征线进行相机位姿估计的基本模型，w和c分别代表世界坐标系和当前相机的相机坐标系，m个三维特征点
所述点约束和线约束的几何采用矩阵形式表示，如下：
对第i个空间点，即第i个三维特征点，构建点约束{pgci}：
对第j个线约束{lgcj}：
本发明中，t和r分别表示从世界坐标系向相机坐标系的平移向量和旋转矩阵。
内参矩阵k如下：
内参矩阵k包含5个内部参数，所述内部参数包括焦距、图像传感器尺寸和主点。其中，内部参数αx＝f·mx，αy＝f·my，αx和αy表示以像素衡量的焦距，mx和my分别表示像素大小到实际距离的尺度因子，f是以距离衡量的实际焦距。γ表示影像平面的x坐标和y坐标之间的偏度系数，通常为0；u0和v0表示影像平面的像主点的坐标。
步骤102，根据特征点和特征线的几何约束构造超定线性系统，即联立步骤101中所有的点约束和线约束构造统一的方程组。
步骤103，根据超定线性系统的解法求解平移向量t关于旋转矩阵r的表达式，将问题转化为求解旋转矩阵r的问题。
超定线性系统的解法如下：
对超定方程at＝b，其中，a是系数矩阵，b是常数项。可按照如下公式解得t：
t＝(ata)-1atb(2)
将步骤102中超定线性系统带入此公式(2)，即可得t关于r的表达式，即可将同时求t和r问题转化为只求r的问题。
步骤104，根据点约束和线约束定义子代价函数，并根据最小二乘法构建总代价函数。
将点约束{pgci}分开记为pgci,1和pgci,2，分别代表点约束{pgci}的两个相互独立的子约束。同理，线约束{lgcj}可以记为lgci,1和lgci,2，分别代表线约束{lgcj}的两个相互独立的子约束。
点约束和线约束均包括三个子约束，该三个子约束中，有两个相互独立的子约束，另一子约束则是根据该两个相互独立的子约束获得。pgci,1和pgci,2、以及lgci,1和lgci,2则为三个子约束中的两个相互独立的子约束。
则可以分别构建点约束和线约束的子代价函数，如下：
所述第i个特征点的点约束子代价函数fi(r,t)为：
所述第j条特征线的线约束子代价函数gj(r,t)为：
根据最小二乘法，得到总代价函数f(r,t)：
式(5)中：
步骤105，使用已有的凯里参数化方法描述旋转矩阵r，使总代价函数f(r,t)转化为四阶多项式，并去除恒正系数，得到最终的代价函数。
使用凯里参数化，将r写为：
式(6)中，s＝1+b2+c2+d2；b，c，d均为旋转矩阵r的分解参数。
照此方法处理则步骤103中t的表达式，可以改写为
将上述r和t的表达式带入公式(5)的总代价函数f(r,t)中，并由于s＝1+b2+c2+d2恒正，则r和t的表达式中的系数都可以去掉，于是得到最终的代价函数f(s)：
式(7)中，h(s)是具有未知可变向量s＝[b,c,d]t的二次多项式。
步骤106，在一阶最优条件下对步骤105得到的四阶多项式(即代价函数f(s))进行最小化，即令其一阶偏导数等于零，见公式(8)，获得所有驻点，驻点即候选的极小值点。
式(8)中：
sk代表第k个点特征或线特征对应的旋转矩阵分解参数；
ξk(s)表示f(s)对sk求偏导所得的三次多项式。
ξk(s)等于零时未知数b,c,d的取值即驻点。
由ξk(s)构成的多项式系统可以写为vθ＝0，其中，v是由ξk(s)组成的线性方程组矩阵化表示的3×20系数矩阵，其维度不受点特征和线特征数目的影响，始终保持为3×20，θ＝[b3,b2c,...,b,c3,c2d,...,c,d3,d2,d,1]t，是由r的分解参数构成的向量。
步骤107，应用
经过以上步骤101～106，相机位姿估计问题已经被转化为了求解多变量多项式系统的问题，其解决方案可以使用
步骤2，对步骤1中得到的相机位姿初始旋转矩阵r和平移向量t进行优化，以获得更加精确的相机位姿。
本步骤包括以下子步骤：
步骤201，将点约束和线约束重写为新的统一形式：
当给定ω＝m+n组点特征和线特征时，可以将{pgci}和{lgcj}重写为统一的形式：
其中：
ψω由观察值ηω组成；ηω是一个由特征线两个端点组成的一个2*2的二维坐标矩阵，对于检测到的每个特征点和每条特性线的两端点，真实数值
x是包含来自r和t的整个元素的未知12维向量，即x＝[r1,r2,...,r9,t1,t2,t3]t，其中r1,r2,...,r9表示旋转矩阵的9个元素，t1,t2,t3表示平移向量的三个元素。
由上同时也有：
式(10)中：
η＝ηω代表当变量η取值为ηω时偏导数的值；
在近似满足高斯分布下，
步骤202，通过引入桑普森误差建立适合在fns迭代方法之下能够迅速收敛的目标函数，并通过fns迭代器得到结果。
当ψω近似为高斯分布时，最小化ψ空间中马氏距离的公式如下：
式(11)中，ω＝m+n表示点特征和线特征数目的总和，
使用拉格朗日乘数消除上方程中额外的约束，问题可以简化为最小化如下的无约束方程的问题：
式(12)中：
h(x)则是在公式(11)中引入的sampson误差的形式，它可以通过对x求导来进行最小化：
式(13)中，m和l都是取决于x的12×12矩阵。
上式中使用了基本数值方案(fns)的迭代方法来求解x，对于具有较小的m-l特征值的特征向量是每次迭代的x的新解，并且m和l应该被最新的x更新直到收敛。
虽然该方法在理论上不需要具体的初始值，但是为了使迭代更快地收敛，可用统一模型的输出来初始化它。
步骤203，对迭代后的结果进行奇异值分解来微调所得的经过优化的旋转矩阵r。
由于该方法不考虑r的正交或决定性约束，因此需要在迭代后进行后验校正，通过奇异值分解(svd)微调所得解r
步骤204，旋转矩阵r带入步骤1中得到的t关于r的表达式式中，得到经过优化的平移向量t。
具体实施时，本发明方法可基于软件技术实现自动运行流程，也可采用模块化方式实现相应系统。因此，本发明相应提供的一种基于特征点线的单目相机位姿估计和优化系统，包括以下模块：
第一模块，用来基于m个特征点和n条特征线的几何约束构建代价函数，利用代价函数求解相机位姿的旋转矩阵r和平移向量t，其中，m+n≥3；
所述第一模块进一步包括子模块：
第一子模块，用来基于几何约束分别建立各特征点矩阵形式的点约束和各特征线矩阵形式的线约束；
第二子模块，用来联立所有的点约束和线约束构造超定线性系统；
第三子模块，用来求解超定线性系统，获得t关于r的表达式；
第四子模块，用来根据最小二乘法构建总代价函数f(r,t)：
其中：fi(r,t)表示第i个点约束的子代价函数，
第五子模块，用来引入凯里参数化描述r，使f(r,t)转化为四阶多项式，得到最终的代价函数f(s)；
第六子模块，用来令f(s)分别对各特征点和各特征线对应r的分解参数矩阵的一阶偏导为0，计算驻点；
第七子模块，用来应用
第二模块，用来对第一模块获得的旋转矩阵r和平移向量t进行优化；
所述第二模块进一步包括：
第八子模块，用来将所有的点约束和线约束表示为
第九子模块，用来对
第十子模块，用来对优化后的r进行奇异值分解以微调r；
第十一子模块，用来根据微调后的r和第三子模块所获得的t关于r的表达式，计算优化后的t。
需要强调的是，本发明所述的实施例是说明性的，而不是限定性的。因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。