曼哈顿世界是符合曼哈顿假设(Coughlan和Yuille，2002)的结构化场景的简称，具有强烈的结构规律性，符合笛卡儿坐标系，可引申出3个主导方向即对应三正交消失点。消失点(vanishing point，VP)作为直线的潜在观测，是一种全局信息，可以显式地表示相机坐标系与世界坐标系之间的姿态关系。引入消失点有利于在曼哈顿世界环境中更加准确和鲁棒地完成位姿估计、3维重建、场景分割及识别等任务。近年来，针对消失点的研究愈来愈多，基于单目图像的消失点估计方法仅需要利用单目视觉传感器。从实时性、准确性和鲁棒性等方面考虑，目前基于随机采样一致性(random sample consistency，RANSAC)消失点估计方法性能最优，但RANSAC的本质是共识集最大化，无法保证估计结果的最优性。本文以经典的基于RANSAC的方法为基础，提出使用非线性优化方法得到最优消失点估计。

曼哈顿世界下的消失点估计问题得到了众多学者的关注，提出了许多算法。除个别算法(Coughlan和Yuille，2002)直接考虑图像中像素点的强度梯度，大多数消失点估计方法依赖于图像中检测到的线段。以已知图像内一组直线特征为前提，找出最优消失点估计，即得到使直线分类后对应到三正交消失点的内点直线(inliers)比例最高的三正交VP。基于提取直线的消失点估计算法分为4类(Bazin和Pollefeys，2012；孔祥龙，2017)，其中基于霍夫变换(Hough和Paul，1962)的算法提出较早，因未采用曼哈顿世界下的消失点正交性约束，会导致消失点次优估计。基于期望最大化(expectation maximization，EM)(Dempster等，1977)的算法(Wildenauer和Vincze，2007；Denis等，2008)因最大化迭代优化导致计算量较大，并且可能会收敛到局部最优解。基于穷举搜索的算法(Rother，2002)采取对所有可能解遍历比较的方式获得最终估计，即便BNB(branch-and-bound algorithm)(Bazin等，2012)在搜索迭代时采用分支定界法，计算复杂度依旧非常高，无法满足实时性需求。与上述方法相比，基于RANSAC的消失点估计方法在实时性、精确性和鲁棒性上均有良好表现，得到广泛应用。该方法首先在直线组中任意选择固定数目线元组构建三正交VP假设，然后计算符合该假设的Inliers比例，通过遍历假设，最大化Inliers比例。Tardif(2009)基于RANSAC的变体J-Linkage提出的消失点估计算法，生成假设的平行线类后通过EM算法找到最优消失点。该方法不强制执行消失点正交性约束，故其确定的消失点通常不是正交的。作为改进，RNS(Mirzaei和Roumeliotis，2011)以及Wildenauer和Hanbury(2012)在生成假设过程中利用正交性约束将问题表述为约束最小二乘问题，取得了很好效果。但是，其采用普吕克坐标表示空间直线方向，存在参数冗余而不利于非线性优化。Zhang等人(2016)提出的RCM(R3_CM1)和RCMI(R3_CM1_Iter)算法基于提取到的直线端点坐标，对空间直线方向进行单参数化，在求解效率及求解精度方面均表现优异。此类方法具有非确定性，可获得较准确的结果，但无法保证结果的最优性。

考虑到基于RANSAC的算法具有的优势，本文集成正交约束和单参数法获得初始估计后，引入非线性优化来解决其非最优的缺陷，最终得到基于非线性优化的消失点估计算法。

如图 1所示，一组平行空间直线(${\mathit{\boldsymbol{L}}}_{1}，{\mathit{\boldsymbol{L}}}_{2}，{\mathit{\boldsymbol{L}}}_{3}$)延伸到视野无穷远处在成像平面上交汇为一点即${\mathit{\boldsymbol{l}}}_{1}、{\mathit{\boldsymbol{l}}}_{2}、{\mathit{\boldsymbol{l}}}_{3}$交于点$VP$，该点即消失点。由相机中心指向消失点的向量即为表征平行空间直线的方向的消失方向。消失点仅与相机的旋转相关，与平移无关。消失点与消失方向具有相同的几何特性，为方便计算，本文中，消失点均指代相机坐标系下归一化的消失方向。

本文算法首先借助基于RANSAC的消失点估计方法为后续优化提供初始估计。利用图像直线端点坐标及其与空间直线间的对应关系实现空间直线参数化，然后在RANSAC过程中利用正交约束构建候选假设，多次迭代得到最终估计。

本文算法适用于针孔相机模型。利用针孔成像模型，在已知相机内参数矩阵${\mathit{\boldsymbol{K}}}$前提下，通过欧拉坐标变换中像素点${\mathit{\boldsymbol{p}}}$与相机坐标系下空间坐标点${\mathit{\boldsymbol{P}}}_{c}$间的关系，得到空间点${\mathit{\boldsymbol{P}}}_{c}$在归一化平面上的投影点在相机坐标系下的空间坐标${\mathit{\boldsymbol{\bar P}}}_{c}$。具体为

$ s \boldsymbol{p} =\boldsymbol{K} \boldsymbol{P}_{c} $

(1)

$ \overline{\boldsymbol{P}}_{c} =\boldsymbol{P}_{c} / s $

(2)

式中，$s$代表尺度信息，采用齐次坐标即${\mathit{\boldsymbol{P}}}_{c}$的深度值。

设图像中检测到的直线在像素坐标系下表示为${\mathit{\boldsymbol{l}}}=[{\mathit{\boldsymbol{p}}}_{s}，{\mathit{\boldsymbol{p}}}_{e}]=[(u_{s}, v_{s})，(u_{e}, v_{e})]$，对应于相机坐标系下空间直线${\mathit{\boldsymbol{L}}}=[{\mathit{\boldsymbol{P}}}_{c, s}，{\mathit{\boldsymbol{P}}}_{c, e}]$，如图 2直线投影所示，归一化处理后对应于${\mathit{\boldsymbol{\bar L}}}=[{\mathit{\boldsymbol{\bar P}}_{c, s}}，{\mathit{\boldsymbol{\bar P}}}_{c, e}]=[(x_{s}, y_{s})，(x_{e}, y_{e})]$。此外，${\mathit{\boldsymbol{n}}}_{c}$为直线与相机中心所构成的投影平面(图 2中虚线平面${\mathit{\boldsymbol{O}}}_{c}{\mathit{\boldsymbol{P}}}_{c, s}{\mathit{\boldsymbol{P}}}_{c, e}$)的法向量，与${\mathit{\boldsymbol{l}}}$、${\mathit{\boldsymbol{L}}}$均为相互垂直关系。

后续相关计算均基于归一化后直线端点的坐标展开。但因两端点深度值未知，无法恢复直线方向。

为构建空间直线方向${\mathit{\boldsymbol{d}}}$即求解$\mathit{\boldsymbol{\bar P}}′_{c, e}$，采用单参数法，引入空间直线两端点的深度的比值$λ$。即

$ \begin{gathered} \boldsymbol{d}=\boldsymbol{O}_{c} \boldsymbol{P}_{c, s}-\boldsymbol{O}_{c} \boldsymbol{P}_{c, e}= \\ \boldsymbol{O}_{c} \overline{\boldsymbol{P}}_{c, s}-\boldsymbol{O}_{c} \overline{\boldsymbol{P}}{}_{c, e}^{\prime}=\left[\begin{array}{c} x_{s}-\lambda x_{e} \\ y_{s}-\lambda y_{e} \\ 1-\lambda \end{array}\right] \end{gathered} $

(3)

曼哈顿世界中对应到不同消失点的空间直线具有正交关系，且对应到同一消失点的具有平行关系。基于数据集提供的像素坐标系下线三元组，可利用直线间相互关系求解直线参数并生成候选假设。

曼哈顿世界中任一线三元组对应的空间直线关系存在以下3种可能：1)3条直线互相正交，即对应到三正交消失点；2)两条直线相互平行，并且与另一条直线相互正交；3)3条直线相互平行。

因求解三正交消失点的前提是图像中至少包含对应到其中两个消失方向的直线组，故第3种情形不予考虑。为简化求解过程并保证实时性，本文算法针对第2种情形进行求解。假设当前处理线三元组$\{{\mathit{\boldsymbol{l}}}_{1}, {\mathit{\boldsymbol{l}}}_{2}, {\mathit{\boldsymbol{l}}}_{3}\}$，3条图像直线分别对应空间直线${\mathit{\boldsymbol{L}}}_{1}，{\mathit{\boldsymbol{L}}}_{2}，{\mathit{\boldsymbol{L}}}_{3}$，归一化处理并利用式(3)单参数化后，得到空间直线方向。分别为

$ \begin{aligned} &\boldsymbol{d}_{1}=\left[x_{1 s}-\lambda_{1} x_{1 e}, y_{1 s}-\lambda_{1} y_{1 e}, 1-\lambda_{1}\right]^{\mathrm{T}} \\ &\boldsymbol{d}_{2}=\left[x_{2 s}-\lambda_{2} x_{2 e}, y_{2 s}-\lambda_{2} y_{2 e}, 1-\lambda_{2}\right]^{\mathrm{T}} \\ &\boldsymbol{d}_{3}=\left[x_{3 s}-\lambda_{3} x_{3 e}, y_{3 s}-\lambda_{3} y_{3 e}, 1-\lambda_{3}\right]^{\mathrm{T}} \end{aligned} $

(4)

设${\mathit{\boldsymbol{d}}}_{1}||{\mathit{\boldsymbol{d}}}_{2}, {\mathit{\boldsymbol{d}}}_{1}⊥{\mathit{\boldsymbol{d}}}_{3}, $，则${\mathit{\boldsymbol{d}}}_{1}×{\mathit{\boldsymbol{d}}}_{2}=0, {\mathit{\boldsymbol{d}}}_{1}·{\mathit{\boldsymbol{d}}}_{3}=0$。即

$ \left\{\begin{array}{l} \left(y_{1 s}-\lambda_{1} y_{1 e}\right)\left(1-\lambda_{2}\right)-\left(y_{2 s}-\lambda_{2} y_{2 e}\right)\left(1-\lambda_{1}\right)=0 \\ \left(x_{2 s}-\lambda_{2} x_{2 e}\right)\left(1-\lambda_{1}\right)-\left(x_{1 s}-\lambda_{1} x_{1 e}\right)\left(1-\lambda_{2}\right)=0 \\ \left(x_{1 s}-\lambda_{1} x_{1 e}\right)\left(y_{2 s}-\lambda_{2} y_{2 e}\right)- \\ \ \ \ \ \ \ \ \ \ \ \ \ \left(x_{2 s}-\lambda_{2} x_{2 e}\right)\left(y_{1 s}-\lambda_{1} y_{1 e}\right)=0 \\ \left(x_{1 s}-\lambda_{1} x_{1 e}\right)\left(x_{3 s}-\lambda_{3} x_{3 e}\right)+ \\ \ \ \ \ \ \ \ \ \ \ \ \ \left(y_{1 s}-\lambda_{1} y_{1 e}\right)\left(y_{3 s}-\lambda_{3} y_{3 e}\right)+ \\ \ \ \ \ \ \ \ \ \ \ \ \ \left(1-\lambda_{1}\right)\left(1-\lambda_{3}\right)=0 \end{array}\right. $

(5)

方程组(5)的前3个方程表示平行关系，它们是相关的，即通过前两个方程可以推导出第3个方程。因此可以只使用前两个方程直接计算出$λ_{1}$和$λ_{2}$，而第3个方程的意义在于可以排除$λ_{1}=λ_{2}=1$的这组平凡解(此时${\mathit{\boldsymbol{L}}}_{1}和{\mathit{\boldsymbol{L}}}_{2}$平行于成像平面)。而$λ_{1}$和$λ_{2}$只需求解出一个即可，因为${\mathit{\boldsymbol{L}}}_{1}和{\mathit{\boldsymbol{L}}}_{2}$对应同一个消失点。$λ_{1}$的求解结果为

$ \begin{gathered} \lambda_{1}= \\ \frac{x_{2 s} y_{2 e}-x_{1 s} y_{2 e}-x_{2 s} y_{1 s}-x_{2 e} y_{2 s}+x_{2 e} y_{1 s}+x_{1 s} y_{2 s}}{x_{2 s} y_{2 e}-x_{1 e} y_{2 e}-x_{2 s} y_{1 e}-x_{2 e} y_{2 s}+x_{2 e} y_{1 e}+x_{1 e} y_{2 s}} \end{gathered} $

(6)

求得$λ_{1}$后，可通过方程组(5)的第4个方程的正交约束求解$λ_{3}$，具体为

$ \begin{gathered} \lambda_{3}= \\ \frac{\left(x_{1 s}-\lambda_{1} x_{1 e}\right) x_{3 s}+\left(y_{1 s}-\lambda_{1} y_{1 e}\right) y_{3 s}+\left(1-\lambda_{1}\right)}{\left(x_{1 s}-\lambda_{1} x_{1 e}\right) x_{3 e}+\left(y_{1 s}-\lambda_{1} y_{1 e}\right) y_{3 e}+\left(1-\lambda_{1}\right)} \end{gathered} $

(7)

最终可解得三正交消失点，具体为

$ \begin{aligned} &\boldsymbol{V P}_{c}=\left[\boldsymbol{V P}_{c 1}, \boldsymbol{V P}_{c 2}, \boldsymbol{V P}_{c 3}\right]= \\ &{\left[\frac{\boldsymbol{d}_{1}}{\left\|\boldsymbol{d}_{1}\right\|}, \frac{\boldsymbol{d}_{3}}{\left\|\boldsymbol{d}_{3}\right\|}, \frac{\boldsymbol{d}_{1}}{\left\|\boldsymbol{d}_{1}\right\|} \times \frac{\boldsymbol{d}_{3}}{\left\|d_{3}\right\|}\right]} \end{aligned} $

(8)

上述三正交消失点在实际提取时，一般采用图像坐标系确定3个方向，即${\mathit{\boldsymbol{VP}}}_{c1}$对应$X$(右)方向，${\mathit{\boldsymbol{VP}}}_{c2}$对应$Y$(下)方向，${\mathit{\boldsymbol{VP}}}_{c3}$对应$Z$(前)方向。多次估计时，利用小旋转假设使新提取消失点与第1帧确定的消失点方向相符合，便于对消失点的连续跟踪。

对于没有任何先验信息的线三元组，如通过随机选取3条直线构造线三元组，遍历所有组合，并对3种情况进行讨论，则所得假设存在$3{\mathit{\boldsymbol{C}}}^{3}_{n}$个，计算量大并耗时增加。本文利用图像直线的斜率分布规律建立图像直线的斜率统计直方图，在构建线三元组时，在峰值区间中抽取两条构成平行关系，再另任选一条构成正交关系，以有效提高估计效率。

利用RANSAC过程中每一候选假设对直线进行分类时，依据以下误差度量进行判断：

如图像直线${\mathit{\boldsymbol{l}}}_{j}$对应于消失点${\mathit{\boldsymbol{VP}}}_{ci}$，则直线投影平面(图 2中虚线平面)的法向量${\mathit{\boldsymbol{n}}}_{j}$与消失点相互垂直。考虑到消失点估计误差，两者并不严格满足垂直关系。设$τ$为角度偏差阈值，如该直线对应于消失点${\mathit{\boldsymbol{VP}}}_{ci}$，则应满足

$ \left|\arccos \left(\boldsymbol{V} \boldsymbol{P}_{c i} \times \boldsymbol{n}_{j}\right)-\frac{{\rm{ \mathsf{ π} }}}{2}\right|<\tau, i \in\{1,2,3\} $

(9)

$ \Delta=\boldsymbol{V} \boldsymbol{P}_{c i}^{\mathrm{T}} \boldsymbol{n}_{j} $

(10)

计算式(10)，Δ对应内点阈值取为$Δ_{\rm{Thes}}=0.03$，即对应于$τ≈1.72°$。如该直线归类于${\mathit{\boldsymbol{VP}}}_{ci}$，则将该直线投影平面法向量记为${\mathit{\boldsymbol{n}}}_{i, j}$。

计算误差完成对直线的分类后，即可得到每一消失点候选假设对应的内点比例$δ$。经RANSAC过程多次迭代，选择具有最高比例$δ$的候选假设作为最终估计结果。

因为RANSAC方法只能找出抽取样本得到的测试集中的最优结果，故获得的消失点存在对应到每条直线的误差。为进一步提高消失点估计精度，本文提出利用直线分类时计算的误差度量构建最小二乘优化模型，并用非线性优化算法求解。具体为

$ \arg \min \limits_{\theta} F(\theta)=\arg \min \limits_{\theta}\left(\boldsymbol{e}^{\mathrm{T}} \boldsymbol{W} \boldsymbol{e}\right) $

(11)

式中，$θ$指待优化参数，${\mathit{\boldsymbol{e}}}$为误差项，${\mathit{\boldsymbol{W}}}$为对应的权重矩阵。

考虑到最小二乘迭代优化易受外点影响，利用RANSAC方法得到的三正交消失点对直线的分类结果初筛，仅对分类得到的内点直线集$\{{\mathit{\boldsymbol{C}}}_{1}, {\mathit{\boldsymbol{C}}}_{2}, {\mathit{\boldsymbol{C}}}_{3}\}$中的内点进行非线性最小二乘优化。

根据误差度量构建非线性最小二乘优化模型，首先明确对应到消失点${\mathit{\boldsymbol{VP}}}_{ci}$的直线集${\mathit{\boldsymbol{C}}}_{i}$中第$j$条直线的误差项${\mathit{\boldsymbol{e}}}_{i, j}$。具体为

$ \begin{gathered} \boldsymbol{e}_{i, j}=\boldsymbol{V} \boldsymbol{P}_{ci}{ }^{\mathrm{T}} \boldsymbol{n}_{i, j}=\left(\boldsymbol{V} \boldsymbol{P}_{c} \boldsymbol{I}_{i}\right)^{\mathrm{T}} \boldsymbol{n}_{i, j}= \\ \boldsymbol{I}_{i}^{\mathrm{T}} \boldsymbol{V} \boldsymbol{P}_{c}^{\mathrm{T}} \boldsymbol{n}_{i, j}=\boldsymbol{I}_{i}^{\mathrm{T}} \boldsymbol{R} \boldsymbol{n}_{i, j} \\ i=1,2,3, j \in {\bf{N}}^{+} \end{gathered} $

(12)

式中，将直线投影平面法向量信息${\mathit{\boldsymbol{n}}}_{i, j}$看做输入，${\mathit{\boldsymbol{I}}}$为三阶单位矩阵，待估计参数值即旋转矩阵${\mathit{\boldsymbol{R}}}={\mathit{\boldsymbol{VP}}}^{{\rm T}}_{c}$，由${\mathit{\boldsymbol{C}}}_{i}$($i=1, 2, 3$)中误差项${\mathit{\boldsymbol{e}}}_{i, j}$构成${\mathit{\boldsymbol{e}}}_{i}=[{\mathit{\boldsymbol{e}}}_{i, 1}, {\mathit{\boldsymbol{e}}}_{i, 2}, …, {\mathit{\boldsymbol{e}}}_{i, j}]^{{\rm T}}$，最终有误差函数${\mathit{\boldsymbol{e}}}=[{\mathit{\boldsymbol{e}}}^{{\rm T}}_{1}, {\mathit{\boldsymbol{e}}}^{{\rm T}}_{2}, {\mathit{\boldsymbol{e}}}^{{\rm T}}_{3}]^{{\rm T}}$。

采用高斯—牛顿迭代方法进行非线性优化，考虑到方向余弦参数冗余、欧拉角存在万向锁问题，选用四元数${\mathit{\boldsymbol{q}}}$计算雅克比矩阵。将误差函数进行一阶泰勒展开，$Δ{\mathit{\boldsymbol{q}}}$为变化量，对误差函数在${\mathit{\boldsymbol{q}}}$处进行线性化处理。则

$ \mathit{\boldsymbol{e}}(\mathit{\boldsymbol{q}} + \Delta \mathit{\boldsymbol{q}}) = \mathit{\boldsymbol{e}}(\mathit{\boldsymbol{q}}) + \mathit{\boldsymbol{J}}\Delta \mathit{\boldsymbol{q}} $

(13)

式中，雅克比矩阵${\mathit{\boldsymbol{J}}}$是当前图像所有直线的误差函数${\mathit{\boldsymbol{e}}}$关于${\mathit{\boldsymbol{q}}}$的一阶偏导。对每一误差项${\mathit{\boldsymbol{e}}}_{i, j}$，有

$ \frac{\partial \boldsymbol{e}_{i, j}}{\partial \boldsymbol{q}}=\boldsymbol{I}_{i}^{\mathrm{T}}\left(\frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial \boldsymbol{q}} \boldsymbol{n}_{i, j}\right) $

(14)

方向余弦矩阵${\mathit{\boldsymbol{R}}}$与四元数${\mathit{\boldsymbol{q}}}$之间存在一定的转换关系。设四元数${\mathit{\boldsymbol{q}}}=q_{0}+q_{1}{\mathit{\boldsymbol{i}}}+q_{2}{\mathit{\boldsymbol{ j}}}+q_{3}{\mathit{\boldsymbol{k}}}$，解得对方向余弦矩阵${\mathit{\boldsymbol{R}}}({\mathit{\boldsymbol{q}}})$的偏导。具体为

$ \begin{gathered} \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial \boldsymbol{q}}=\left[\frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{0}} \quad \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{1}} \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{2}} \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{3}}\right] \\ \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{0}}=\left[\begin{array}{ccc} 0 & -2 q_{3} & 2 q_{2} \\ 2 q_{3} & 0 & -2 q_{1} \\ -2 q_{2} & 2 q_{1} & 0 \end{array}\right] \\ \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{1}}=\left[\begin{array}{ccc} 0 & 2 q_{2} & 2 q_{3} \\ 2 q_{2} & -4 q_{1} & -2 q_{0} \\ 2 q_{3} & 2 q_{0} & -4 q_{1} \end{array}\right]\\ \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{2}}=\left[\begin{array}{ccc} -4 q_{2} & 2 q_{1} & 2 q_{0} \\ 2 q_{1} & 0 & 2 q_{3} \\ -2 q_{0} & 2 q_{3} & -4 q_{2} \end{array}\right] \\ \frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{3}}=\left[\begin{array}{ccc} -4 q_{3} & -2 q_{0} & 2 q_{1} \\ 2 q_{0} & -4 q_{3} & 2 q_{2} \\ 2 q_{1} & 2 q_{2} & 0 \end{array}\right] \end{gathered} $

(15)

最终得误差项${\mathit{\boldsymbol{e}}}_{i, j}$对四元数${\mathit{\boldsymbol{q}}}$的偏导。对所有参与优化的直线计算偏导数，最终得到雅克比矩阵${\mathit{\boldsymbol{J}}}$。

$ \frac{\partial \boldsymbol{e}_{i, j}}{\partial \boldsymbol{q}}=\left[\begin{array}{l} \boldsymbol{I}_{i}^{\mathrm{T}}\left(\frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{0}} \boldsymbol{n}_{i, j}\right) \\ \boldsymbol{I}_{i}^{\mathrm{T}}\left(\frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{1}} \boldsymbol{n}_{i, j}\right) \\ \boldsymbol{I}_{i}^{\mathrm{T}}\left(\frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{2}} \boldsymbol{n}_{i, j}\right) \\ \boldsymbol{I}_{i}^{\mathrm{T}}\left(\frac{\partial \boldsymbol{R}(\boldsymbol{q})}{\partial q_{3}} \boldsymbol{n}_{i, j}\right) \end{array}\right]^{\rm{T}} $

(16)

利用雅克比矩阵${\mathit{\boldsymbol{J}}}$将非线性优化函数重写为

$ \begin{gathered} \arg \min \limits_{\boldsymbol{q}} E(\boldsymbol{q}+\Delta \boldsymbol{q})= \\ \arg \min \limits_{\boldsymbol{q}} \boldsymbol{e}(\boldsymbol{q}+\Delta \boldsymbol{q})^{\mathrm{T}} \boldsymbol{W} \boldsymbol{e}(\boldsymbol{q}+\Delta \boldsymbol{q}) \approx \\ \arg \min \limits_{\boldsymbol{q}}(\boldsymbol{e}(\boldsymbol{q})+\boldsymbol{J} \Delta \boldsymbol{q})^{\mathrm{T}} \boldsymbol{W}(\boldsymbol{e}(\boldsymbol{q})+\boldsymbol{J} \Delta \boldsymbol{q})= \\ \arg \min \limits_{\boldsymbol{q}}\left(\boldsymbol{e}^{\mathrm{T}}(\boldsymbol{q}) \boldsymbol{W} \boldsymbol{e}(\boldsymbol{q})+2 \Delta \boldsymbol{q}^{\mathrm{T}} \boldsymbol{J}^{\mathrm{T}} \boldsymbol{W} \boldsymbol{e}(\boldsymbol{q})+\right. \\ \left.\Delta \boldsymbol{q}^{\mathrm{T}} \boldsymbol{J}^{\mathrm{T}} \boldsymbol{W} \boldsymbol{J} \boldsymbol{\Delta} \boldsymbol{q}\right) \end{gathered} $

(17)

求解上式关于$Δ{\mathit{\boldsymbol{q}}}$的导数，解得使目标函数取得最优解的优化增量，即可更新参数估计。具体为

$ \Delta \boldsymbol{q}=-\left(\boldsymbol{J}^{\mathrm{T}} \boldsymbol{W} \boldsymbol{J}\right)^{-1} \boldsymbol{J}^{\mathrm{T}} \boldsymbol{W} \boldsymbol{e}(\boldsymbol{q}) $

(18)

$ \boldsymbol{R}(\boldsymbol{q})_{\mathrm{new}}=\boldsymbol{R}(\Delta \boldsymbol{q}) \boldsymbol{R} $

(19)

采用上述方法经多次迭代最终得到最优参数估计${\mathit{\boldsymbol{\hat R}}}({\mathit{\boldsymbol{q}}})$，即得到最优消失点${\mathit{\boldsymbol{\widehat {VP}}}}_{c}$。即

$ \widehat{\boldsymbol{V P}}_{c}=\hat{\boldsymbol{R}}{}^{\mathrm{T}}(\boldsymbol{q}) $

(20)

值得注意的是，在迭代优化过程中需要时刻保持四元数${\mathit{\boldsymbol{q}}}$的单位性，因此需要进行归一化操作，否则可能导致迭代优化过程的发散。

非线性优化时使用直线分类结果中的内点，但考虑到过分依赖误差较大的外点或直接忽视误差较大的内点，均可能会造成负优化，得到错误的局部最优解。故本文引入鲁棒核函数以提高优化的鲁棒性(Yu等，2018)。选用Huber核函数作为非线性最小二乘优化模型中的权重矩阵，将原误差平方项替换为较为平滑的函数，并保证单调递增性质以及光滑性质。

针对曼哈顿世界下的消失点估计，三正交消失点中有一消失点与重力加速度方向平行，称其为垂直消失点。当有惯性测量元件(inertial measurement unit, IMU)辅助或运动模型已知可得到IMU坐标系下重力加速度方向时，利用相机和惯性测量元件的外参标定结果，即可获得相机坐标系下重力加速度${\mathit{\boldsymbol{g}}}_{c}$，即垂直消失点，为三正交消失点提供先验约束(孔祥龙，2017)。

利用该先验约束，首先可在计算候选假设评价值与当前最优结果比较前进行初筛。因存在一消失点与重力方向向量夹角接近于0，计算向量叉乘，设定容错阈值$τ_{2}$，则有

$ \left|\min \left\{\arccos \left(\frac{\boldsymbol{V} \boldsymbol{P}_{c i} \times \boldsymbol{g}_{c}}{\left\|\boldsymbol{g}_{c}\right\|}\right)\right\}\right|<\tau_{2}, i=1,2,3 $

(21)

其次，在优化阶段，将问题转换为针对余下两消失点方向对应直线的误差函数的优化。设垂直消失点${\mathit{\boldsymbol{VP}}}_{c2}={\mathit{\boldsymbol{g}}}_{c}$已知，仅优化${\mathit{\boldsymbol{e}}}_{1}$和${\mathit{\boldsymbol{e}}}_{3}$，为减少优化变量，${\mathit{\boldsymbol{e}}}_{3}$对应的消失点有${\mathit{\boldsymbol{VP}}}_{c3}={\mathit{\boldsymbol{VP}}}_{c1}×{\mathit{\boldsymbol{VP}}}_{c2}$，$W^{i}_{jj}$为${\mathit{\boldsymbol{W}}}$中对应直线集${\mathit{\boldsymbol{C}}}_{i}$中第$j$条直线的权重系数，得到一个带约束的最小二乘优化问题，具体为

$ \begin{array}{c} \arg \min \limits_{\boldsymbol{V} \boldsymbol{P}_{c 1}} f\left(\boldsymbol{V} \boldsymbol{P}_{c 1}\right)=\sum\limits_{C_{1}} W_{j j}^{1}\left(\boldsymbol{V} \boldsymbol{P}_{c 1}^{\mathrm{T}} \boldsymbol{n}_{i, j}\right)^{2}+ & \\ \sum\limits_{C_{3}} W_{j j}^{3}\left(\left(\boldsymbol{V} \boldsymbol{P}_{c 1} \times \boldsymbol{V} \boldsymbol{P}_{c 2}\right)^{\mathrm{T}} \boldsymbol{n}_{3, j}\right)^{2} \\ \text { s.t. } \ \ \left(\boldsymbol{V} \boldsymbol{P}_{c 1}\right)^{\mathrm{T}} \boldsymbol{V} \boldsymbol{P}_{c 1}=1, \boldsymbol{V} \boldsymbol{P}_{c 1} \times \boldsymbol{g}_{c}=0 \end{array} $

(22)

采用拉格朗日乘子法(Lagrange multiplier method)进行求解，使用第1个约束构造拉格朗日方程后, 对结果进行施密特正交化以满足第2个约束。即

$ \begin{gathered} \arg \min \limits_{\boldsymbol{VP}_{c 1}} \mathrm{~F}\left(\boldsymbol{V} \boldsymbol{P}_{c 1}\right)=f\left(\boldsymbol{V} \boldsymbol{P}_{c 1}\right)+ \\ \lambda\left(\left(\boldsymbol{V} \boldsymbol{P}_{c 1}\right)^{\mathrm{T}} \boldsymbol{V} \boldsymbol{P}_{c 1}-1\right)= \\ \boldsymbol{V} \boldsymbol{P}_{c 1}^{\mathrm{T}}\left(\sum\limits_{C_{1}} W_{j j}^{1}\left(\boldsymbol{n}_{1, j} \boldsymbol{n}_{1, j}^{\mathrm{T}}\right)\right) \boldsymbol{V} \boldsymbol{P}_{c 1}+ \\ \left(\boldsymbol{V} \boldsymbol{P}_{c 1} \times \boldsymbol{V} \boldsymbol{P}_{c 2}\right)^{\mathrm{T}}\left(\sum\limits_{C_{3}} W_{j j}^{3}\left(\boldsymbol{n}_{3, j} \boldsymbol{n}_{3, j}^{\mathrm{T}}\right)\right)\left(\boldsymbol{V} \boldsymbol{P}_{c 1} \times \boldsymbol{V} \boldsymbol{P}_{c 2}\right)+ \\ \lambda\left(\left(\boldsymbol{V} \boldsymbol{P}_{c 1}\right)^{\mathrm{T}} \boldsymbol{V} \boldsymbol{P}_{c 1}-1\right) \end{gathered} $

(23)

对拉格朗日方程求导后，通过特征值分解方法求解，最终可得到先验约束下的消失点最优估计。即

$ \begin{gathered} \lambda \boldsymbol{V} \boldsymbol{P}_{c 1}=\left(\sum\limits_{C_{1}} W_{j j}^{1}\left(\boldsymbol{n}_{1, j} \boldsymbol{n}_{1, j}^{\mathrm{T}}\right)\right) \boldsymbol{V} \boldsymbol{P}_{c 1}+ \\ {\left[\boldsymbol{V} \boldsymbol{P}_{c 2} \times\right]^{\mathrm{T}}\left(\sum\limits_{C_{3}} W_{j j}^{3}\left(\boldsymbol{n}_{3, j} \boldsymbol{n}_{3, j}^{\mathrm{T}}\right)\right)\left(\boldsymbol{V} \boldsymbol{P}_{c 1} \times \boldsymbol{V} \boldsymbol{P}_{c 2}\right)} \end{gathered} $

(24)

基于MATLAB的SLAM toolbox(Solà等，2012)设计仿真模型，并在YUD(York urban city database)公开数据集(Denis等，2008)上进行仿真实验。对输入图像分别采用基于RANSAC的RCM及其优化算法RCMI(2016)和本文基于非线性优化改进方法(ours)、有先验情况下改进方法(ours + prior)进行消失点估计。同时，设计了RCM + prior对先验信息约束下消失点估计效果进行对比。对消失点估计结果转换为轴角后计算旋转角的角度偏差进行统计分析，验证本文提出方法能否提高消失点估计的准确度。实验在16 GB RAM和2.6 GHz Intel®Core-6700HQ CPU的笔记本电脑上完成，实验代码https://github.com/nubot-nudt/ManhattanVPE。

仿真实验可以准确有效地对算法估计结果和真值进行对比。实验时基于MATLAB SLAM toolbox搭建仿真模型。如图 3所示，其包含80条直线并符合曼哈顿世界假设。设定虚拟机器人以半径为5 m、步长0.8 m、偏航角增量0.09°围绕大小为3 m×3 m×2 m的仿真模型移动，获取各时刻虚拟相机捕捉到的大小为640×480像素的图像，如图 4所示。

建立世界坐标系、机器人坐标系以及相机坐标系间变换关系，获得每次运动后图像中的消失点真值；记录每帧图像中的直线信息，并对直线两端点坐标引入方差为0.5的高斯噪声。

图 5展示了仿真实验中本文算法部分VP提取结果。红、绿、蓝色分别表示$X$、$Y$、$Z$方向消失点。根据2.2节计算误差度量，对误差大于阈值的直线，即局外线采用黄色标识。

首先对直线分类结果进行分析，各方法均能有效进行直线分类。实验结果显示，RCMI估计结果与RCM相同，并无优化效果。因设定的虚拟机器人的运动模式仅改变了偏航角，垂直消失点即对应竖直方向，则同时验证针对有先验情况提出的优化方法的有效性。

不同方法消失点估计结果与真值间的旋转角角度偏差结果及角度偏差累积结果图如图 6和图 7所示。对数据进行具体分析，可得RCM方法估计的VP即${\mathit{\boldsymbol{VP}}}_{c}$与真值间的平均角度偏差为0.73°，本文(ours)优化后平均角度偏差稳定在0.55°, 角度偏差减小了24.6%。ours + prior结果准确度提升了一个数量级，平均角度偏差为0.063 8°，并且所有消失点估计结果角度偏差均小于0.2°，RCM + prior平均角度偏差为0.064°。虽然两者由RCM、ours提供的初始估计不同，但相同的图像直线信息、近似的直线初始分类使两者利用先验信息约束优化结果近似，均能取得较好优化结果并大幅提升消失点估计精度。

为验证本文算法的非线性优化环节并不影响消失点估计的实时性，对仿真实验中各算法的运行时间进行统计分析，各算法10次实验平均耗时如表 1所示。可以看出，本文算法在一帧图像上的平均运行时间为0.016 s，与RCMI耗时近似。其中，平均优化时间为0.008 s，对应每帧图像平均进行3次迭代优化。RCM + prior和ours + prior因引入先验信息约束进行相同的优化，平均优化耗时相近，两者与RCM耗时相差较小。改进后的消失点估计方法整体耗时仍满足实时性要求。

为进一步验证本文算法的性能，基于YUD公开数据集进行实验。该公共数据集主要为纽约大学和多伦多市区中的室内、外场景，共102幅640 × 480像素的图像。实验中使用的直线信息即端点坐标由该数据集提供，均来自人工标注。

图 8为本文算法消失点估计的部分结果，红、绿、蓝和黄色分别表示$X$、$Y$、$Z$方向和局外线。图 9为对图 8中第1幅图像(P1040829)采用不同方法进行消失点估计的对比效果。可以看出，由于消失点本身的鲁棒性，各种方法都具有较小的估计误差，本文方法在3个消失点方向上均实现了有效优化，更加接近真值。图 9的放大区域中，RCMI对RCM没有优化效果，因此二者结果重合。

对所有图像所得结果进行统计分析，发现估计得到的消失点与所给真值间的角度偏差普遍大于仿真环境下实验结果，因为该数据集的直线为人工标注结果，为亚像素精度，使用Collins和Weiss(1990)采用的高斯球体方法和非线性搜索分别估计出真实直线信息和相机内参信息后，独立估计3个消失点方向后进行正交化得到三正交消失点，即其真值也具有一定的误差。

最后，对实验结果进行定量分析。图 10为各消失点估计方法在YUD数据集上的结果角度偏差。可以看出，使用RCM方法得到的VP与数据集真值间平均角度偏差为1.36°，采用RCMI所得结果的平均角度偏差为1.30°，而本文方法结果为0.99°，相较RCM和RCMI平均角度偏差分别减小了27.2%和23.8%，有效优化了消失点估计结果。各消失点估计算法结果在YUD数据集上的角度偏差累积结果图如图 11所示。可以看出，本文方法使80%的消失点估计结果角度偏差在1.5°以内，远远优于原方法及其优化策略，并且更具稳定性。需要指出的是，即使不进行优化，RCM方法基于RANSAC得到的估计结果也具有很高的准确性。

本文对消失点估计方法展开研究，具体分析了基于RANSAC的消失点估计方法，并针对其不足，提出了基于非线性优化的改进方法。利用直线与对应消失点间存在的误差度量构建非线性最小二乘优化模型，采用高斯—牛顿迭代的方法优化，并使用鲁棒核函数提高鲁棒性。同时，考虑有先验信息的情况，构建具有约束的非线性最小二乘优化模型并使用拉格朗日乘子法进行优化求解。最后，分别基于MATLAB SLAM toolbox工具箱构建的仿真环境和YUD数据集进行实验，验证了所提出改进方法的有效性。下一步工作将围绕以下两方面展开：1)在保证准确度的前提下，进一步优化本文算法，提升算法的实时性；2)利用消失点作为全局特征可以表征世界方位信息进而辅助定位这一特性，将消失点为视觉里程计等引入全局约束来抑制定位中累积的姿态误差，提高定位精度。