High-Precision Online Markerless Stereo Extrinsic Calibration1

立体相机和稠密立体视觉已经成为了机器人应用中最为广泛的传感方式之一。高精度的参数标定(包括各摄像机的内参 [intrinsic] 和立体外参 [extrinsic] )是获得高质量、稠密深度测量的关键。一个好的立体标定使立体图像的转换成极线成为平行的。这是大多数密集立体匹配算法的基础,因为稠密立体匹配可以简化为在极线上的一维高效搜索问题。传统上,立体相机采用先进的材料和机械结构进行刚性安装,立体标定采用已知平面图案进行离线标定。然而,由于温度、振动或系统意外冲击的变化,标定参数往往会随着时间的推移而变化。因此,立体相机在线自校准的能力成为机器人使用基于立体的传感方式实现长期自主的关键因素。

1. 预备知识

首先从符号定义开始,观看下图–双目立体对极约束系统:

Illustrator of a stereo epipolar system

定义左右相机矩阵分别为 KR3×3KR3×3 ,假设 KK 是事先通过单目相机标定估计的已知量。假设在左右图像中都观测到一个特征 i ,它位于左右相机光学中心坐标系的坐标分别 xiR3xiR3 ,且它投影到左右图像上坐标分别为 (ui,vi)(ui,vi) 的像素上。而它对应与左右相机坐标系下的深度分别为 λiλi 。假设相机为针孔模型,那么 xixi 对应的归一化像素坐标系为: (1.1)fi=K1[uivi1],fi=K1[uivi1] 其中 xi=λifixi=λifi 。由于左右相机的坐标系可以通过一个旋转矩阵 RR3×3 和一个位移 tR3 进行转换,因此有 (1.2)xi=R xi+t 一个本质矩阵 E(essential matrix)被定义为: (1.3)E=t×R 其中 × 是反对称算子–将一个 R3 向量转换成一个反对称矩阵。对于一个在左相机坐标系的三维空间中沿着 fi 方向的点,它投影到右相机平面的 li 线上,其中线 li 被称为极线。在数学上,极线 li 可以携程本质矩阵 Efi 的乘积: (1.4)li=E fi 而对极约束可以表示为: (1.5)(fi)T li=(fi)T E fi=0 更为详细的对极几何知识可以参考2

2. 算法模型化

由于检测到的特征点位置会受到来至于像素坐标量化与检测性能有限的噪声的影响,因此不能完全满足对极约束。每一对匹配特征 fifi 的对极误差可以定义为: (2.1)(fi)T E fi2 它表示点 fi 到极线 li 在对极几何下的距离平方。我们的目标是求解一个 Rt 使得所有匹配的特征对应的整体极线误差最小: (2.2)minR,ti=0N(fi)Tt×R fi2,s.t.  t=1 单位范数约束 t=1 反应了一个事实,即,本质矩阵 E 有 5 个自由度,3 个用于旋转 RSO(3) ,2 个用于任意尺度缩放的位移 tS2 (式 (1.5) 满足任意比例因子与 t 相乘)。 应用李群李代数原理,在当前旋转估计值 R^ 的正切平面空间参数化一个微小的扰动量 δθR3 ,有: (2.3)RR^(I3+δθ×) 其中, I3 表示为维度为 3×3 的单位矩阵。在当前位移估计值 t^ 的正切平面空间定义一个微小扰动量 δt=[α,β]T ,可以参数化为: (2.4)t=t^+[b1b2]δt=t^+α b1+β b2 其中,b1b2 是切平面内的两个正交基向量,而 αβ 是分别对应于以 b1b2 为基向量的位移尺度因子。如下图所示,

translation unit sphere

由于规定位移 t 在 3D 空间可以任意尺度缩放,因此自由度只有 2 。如上图所示,将 t 约束在一个单位球上,对于当前时刻估计值 t^ 的一个微小扰动可以定义在单位球的切平面上,则有 t=t^+αb1+βb2 。其中,b1b2 是切平面内的两个正交基向量,而 αβ 是分别对应于以 b1b2 为基向量的位移尺度因子

下面介绍基底 b1b2 的求解方式。由于 b1b2 约束在当前时刻估计值 t^ 的切平面上,因此它们都垂直于 t^ 。而且 b1b2 并不是唯一的,可以是任意可以张成切平面的正交基底对。一种方式求解 b1b2 的方式是使得 b1b2t^ 互相正交。可以通过任意选取两个基地 b1b2 ,然后使用 Gram-Schmidt 过程逐个去除相关分量,直到满足正交约束。下面给出该算法过程的伪代码:

Algorithm 1 Projection( u, v )
return u InnerProduct( u, v ) / InnerProduct( u, u )

Algorithm 2 FindingBases( t^ )
idx = 0;
if Absolute( t^(1) ) < Absolute( t^(idx) ) then
idx = 1;
end if
if Absolute( t^(2) ) < Absolute( t^(idx) ) then
idx = 2;
end if

if idx = 0 then
b1={0,1,0}, b2={0,0,1} ;
else if idx = 1 then
b1={1,0,0}, b2={0,0,1} ;
else
b1={1,0,0}, b2={0,1,0} ;
end if

b1=b1Projection(t^,b1) ;
Normalize( b1 );
b2=b2Projection(t^,b2)Projection(b1,b2) ;
Normalize( b2 );
return b1,b2

  • 1~7 行检测 t^ 的主要维度分量
  • 9~15 行根据 t^ 的主要维度分量选择两个预备基底
  • 17~20 行是利用 Gram-Schmidt 过程消除预备基底的关联分量
  • 算法 2 中需要的投影函数在算法 1 里详细介绍

3. 非线性优化

通常情况下,立体相机的外参的先验值可以通过硬件结构亦或是 5 点法算法获取。由此,利用先验信息作为初始值通过在流形 SO(3)×S2 上迭代非线性优化参数求解 ERt 。使用 2. 算法模型化 中提出的参数化去优化方程 (1.5) 。为了更好的表示,定义残差 ri(t,R)=(fi)Tt×Rfi 。那么残差 ri(t,R) 在当前估计值 t^,R^ 处的一阶泰勒展开为: (3.1)ri(t,R)=ri(t^,R^)+Ji(t^,R^)Δ 其中 Δ={δθ,δt}R5 为误差状态向量,Ji(t^,R^) 是残差 ri(t,R) 在当前估计值 t^,R^ 处关联 Δ 雅克比。则有: (3.2)Ji(t^,R^)=[ri(t^,R^)δθri(t^,R^)δt]ri(t^,R^)δθ=(fi)Tt^×R^fi×ri(t^,R^)δt=[(fi)Tb1×R^fi (fi)Tb2×R^fi]

  • ri(t^,R^)δθ 项推导

(3.2a)ri(t^,R^)δθ=limδθ0(fi)Tt^×R^(I+δθ×)fi(fi)Tt^×R^fiδθ=limδθ0(fi)Tt^×R^δθ×fiδθ=limδθ0(fi)Tt^×R^fi×δθδθ=(fi)Tt^×R^fi×

  • ri(t^,R^)δt 项推导

(3.2b)ri(t^,R^)α=limα0(fi)Tt^+αb1+βb2×R^fi(fi)Tt^+βb2×R^fiα=limα0(fi)Tb1×R^fiαα=(fi)Tb1×R^fi同理,ri(t^,R^)β=(fi)Tb2×R^fi

将式 (3.1) 代入式 (2.2) 有: (3.3)minΔi=0N1ri(t^,R^)+Ji(t^,R^)Δ2Δ 求导,使其为 0,得到下面线性方程的解: (3.4)JTJΔ=JTr 其中,J 为雅克比矩阵 Ji(t^,R^) 堆叠形成的雅克比矩阵,r 为残差 ri(t,R) 堆叠形成的残差向量。为了缓解由于特征检测和匹配不完善而产生的异常值的影响,使用 Huber 加权范数对较大残差 ri(t,R) 进行加权: (3.5)wih={1,if  ri(t^,R^)ctctri(t^,R^)otherwise 其中 ct 是一个预定义的 Huber 范数阈值。而数据归一权重 win (在 3.1 归一化对极约束 说明)也将被使用,由此 wi=winwih 。式 (3.4) 可以被修改成加权式: (3.6)JTWJΔ=JTWr 其中 W 是由权重 wi 构成的一个对角矩阵,那么 Δ 的解为: (3.7)Δ=(JTWJ)1JTWr 通过迭代式 (3.7) 并更新下式,直到收敛。 (3.8)R^R^exp(δθ)t^t^+αb1+βb2

3.1 归一化对极约束

在采用迭代优化求解的时候,为了缓解异常值与尺度坐标的不一致,通常采用马氏距离。上文中提到了在优化对极约束时候将采用归一权重 win 使得估计器更为鲁棒。下面将结合论文中提及的参考文献3(相关内容位于该文献的 2.3 小节)进行这部分的介绍。

对极约束(式 (1.5) )给出了图像点对必须满足的唯一必要条件(与深度无关)。因此求解式 (2.2) 从最小化目标函数中获得的运动估计对于常用的图像匹配噪声模型在统计上或几何上不一定是最优的。由此对每个图像点对赋予权重将是需要讨论的问题。在之前的一些文献4中观察到,为了得到更少的偏差估计,对极约束需要适当的归一化。

在透视投影的情况下(假设是针孔模型,其他模型于此类似)。图像点对归一化像素坐标 fifi 的坐标形式 f=[x,y,1]TR3 。设图像匹配点对的检测模型如下: (3.9)fi=f^i+ϵi ,fi=f^i+ηi 其中 f^if^i 是理想的归一化坐标值(无噪声量),ϵi=[ϵi1,ϵi2,0]TR3ηi=[ηi1,ηi2,0]TR3 为检测噪声,而 ϵi1,ϵi2,ηi1,ηi2 设为相同但相互独立符合高斯分布 N(0,σf2) 的随机变量。将式 (3.9) 代入式 (1.5) 有: (3.10)(fi)Tt×R fi=(f^i)Tt×R f^i0+ηiTt×R f^i+(f^i)Tt×R ϵi+ηiTt×R ϵi=ηiTt×R f^i+(f^i)Tt×R ϵi+ηiTt×R ϵiηiTt×R f^i+(f^i)Tt×R ϵi

由于图像坐标 fifi 往往远大于 ϵiηi ,因此去除最后一项微小量。由此可得 (fi)Tt×R fi 是近似高斯分布 N(0,σ2(e3×t×Rfi2+(fi)Tt×Re3×T2)) 独立随机变量。其中 e3=[0,0,1]TR3 。如果我们假设立体相机外参 (R,t) 的先验分布是均匀的,那么 (R,t) 的最大后验(MAP)估计是下面目标函数的全局最小值: (3.11)(R,t)MAPargming(R,t)=argmini=0N1(fi)Tt×R fi2e3×t×Rfi2+(fi)Tt×Re3×T2 这里采用 g(R,t) 表示与对极约束相关的统计归一化目标函数。该目标函数在其它文献中也被称为梯度标准或对极改善。注意,在无噪声的情况下, g(R,t) 就像其非归一化目标函数式 (2.2) 一样达到 0。渐渐的,MAP 估计接近无偏最小均方差估计(MMSE)。因此,一般来说,MAP 估计器给出的偏置估计比非归一化目标函数式 (2.2) 给出的偏置估计要少。而式 (3.11) 中之所以使用 是因为我们在式 (3.10) 中丢掉了微小项。

由此我们可以得到上文中提到的数据归一权重 win 为(其中 e1=[1,0,0]TR3e2=[0,1,0]TR3 ): (3.12)win=1e3×t×Rfi2+(fi)Tt×Re3×T2=1(e1Tt×Rfi)2+(e2Tt×Rfi)2+((fi)Tt×Re1)2+((fi)Tt×Re2)2

4. 立体相机标定外参的协方差估计

该小节将用数学方法推导出 3. 非线性优化 估计外参的协方差。依据式 (3.9) 中假设的模型,且由于 Rt 的协方差等于误差状态 Δ 的协方差,因此,对于优化最终迭代的估计量 t^R^ ,计算式 (3.7) 等式两边的协方差,得: (4.1)ΣΔ=(JTWJ)1JTWΣr((JTWJ)1JTW)TJ1(JTW)1JTWΣr(J1(JTW)JTW)T=J1ΣrJT=(JTΣr1J)1 其中 ΣΔΔ 的协方差,Σrr 的协方差,J1J 的伪逆,(JTW)1JTW 的伪逆。由于检测到的特征是独立,因此 Σr 是一个对角矩阵: (4.2)Σr=[cov(r0(t^,R^))0cov(ri(t^,R^))0cov(rN1(t^,R^))]cov(ri(t^,R^))=[(fi)TE^ΣfE^Tfi+fiTE^TΣfE^fi]E^=t^×R 。其中忽略微小量协方差。如果需要快速计算,还可以进一步简化 ΣΔ 的计算,通过假设所有的协方差 cov(ri(t^,R^)) 都相同(定义为 cr),那么 ΣΔ 可以近似为: (4.3)ΣΔcr(JTJ)1cr(JTWJ)1 由于 JTWJ 是求解式 (3.6) 中的附属品,因此使用上式计算 ΣΔ1 不需要而外求解。从概率论的观点来看,ΣΔ 的最大特征值反应了计算估计的准确性。

5. 工程实现

上文中提出的非线性算法需要一个精心设计的系统流程来支持以获得高精度的标定参数,如下图所示

Overall system pipeline

  1. 对每一幅输入图像,采用 ST 角点检测器5检测图像上的特征点,并使用 BRIEF 关键点描述符6对其进行描述;
  2. 根据 BRIEF 空间中的汉明距进行左右图特征点匹配;
  3. 去除匹配异常值:
    • 利用来自立体相机外参的初始参数根据极线误差去除离群特征匹配点对;
    • 从左图到右图和从右图到左图的匹配需要检测一致性,且在 BRIEF 空间中,最佳匹配的汉明距需要小于第二佳匹配的三分之一。不满足这些唯一匹配检测的点对将去除;
    • 特征匹配需要具有相同的几何模型,如基础矩阵或本质矩阵,应用随机采用一致性进一步过滤异常值;
  4. 特征点对收集时,为了满足实时性以及特征点尽量分布均匀。将图像分割成大小相等的 W×H 个单元,且每个单元中特征点数的上限设为 cm ,而特征点对将根据下面规则进行增加或删除:
    • 如果单元格未满,则保留并添加所有匹配的特征点;
    • 如果单元格已满,则仅保留特征视差差异最大的前 cm 个特征点对,其余的舍弃;
    • 如果单元格已满,且特征视差差异近似,那么随机保留 cm 个特征点对;
  5. 根据上文提及的非线性优化算法进行对立体相机外参进行估计,且判断其协方差的最大特征值是否满足足够小,如果满足的话着标定结束,否则返回 1 。

代码实现:LSXiang/Stereo_Extrinsic_Self-Calibration




Reference


  1. Y Ling · 2019, High-Precision Online Markerless Stereo Extrinsic Calibration ↩︎

  2. Multiple View Geometry in Computer Vision ↩︎

  3. Y. Ma, J. Kosecka, and S. S. Sastry, “Optimization Criteria and Geometric Algorithms for Motion and Structure Estimation,” International Journal of Computer Vision, 2001. ↩︎

  4. J. Weng, T.S. Huang, and N. Ahuja. Optimal motion and structure estimation. IEEE Transactions on Pattern Analysis and Machine Intelligence, 15(9):864–84, 1993. ↩︎

  5. Shi-Tomasi 算法 ↩︎

  6. BRIEF 原理 ↩︎