多种相机模型

现今相机传感器使用广泛,为了使相机成像更加真实,就需要了解对应相机模型以及对应参数,进而修复可能出现的成像问题。了解模型后,采用对应模型进行相机标定,然后去畸变等等。

 

1 针孔相机

1.1 针孔相机模型

对于薄透镜的针孔相机,其成像可简化成如下形式

上面模型中,\((x,y,z)\)为相机坐标系下的3D点,其中以相机镜面正前方为\(z\)方向。上面模型中\(f\)为相机焦距,\(d\)为像距。根据相似三角形原理,不难得到如下结果

(1)$$ \begin{cases}
\dfrac{x}{f}=\dfrac{u}{d-f}
\\
\dfrac{x}{z}=\dfrac{u}{d}
\end{cases}
\Rightarrow f=\dfrac{d}{\dfrac{d}{z}+1} \Rightarrow \dfrac{1}{f}=\dfrac{1}{z}+\dfrac{1}{d}
$$

受限于相机尺寸以及为了扩大成像视野,一般\(z >> d\),这时\(f \approx d\)(这就是成像在焦点附近比较清晰的原因),因此可得到像平面的坐标

(2)$$ 
p_x=f\dfrac{x}{z}
$$

同理可以得到

(3)$$ 
p_y=f\dfrac{y}{z}
$$

注意,\((p_x, p_y)\)仅是理论上像平面的坐标。由于相机一般由感光器件来进行图像映射,感光器反馈的是像素单元的坐标,假设单个像素横向占用\(1/l_x\)米,纵向占用\(1/l_y\)米,则可以使用\(l_x、l_y\)将\((p_x, p_y)\)缩放到像素坐标。另一方面,由于\((p_x, p_y)\)得到的坐标是以相机光心为原点,而常见的图像坐标以图像左上角为原点,因此需要移动原点,这时在缩放后的像素坐标上加一个偏移\((c_x,c_y)\)。一般\((c_x,c_y)\)取相机光心在像素中的坐标值。最终有如下针孔相机模型

(4)$$ \begin{cases}
u =l_xp_x+ c_x= f l_x \dfrac{x}{z}+c_x
\\
v =l_yp_y + c_y =  f l_y \dfrac{y}{z} + c_y
\end{cases}
$$

为了简洁表示,这里设归一化坐标\((x',y')\)如下

(5)$$ \begin{cases}
x'=\dfrac{x}{z}
\\
y'=\dfrac{y}{z}
\end{cases}
$$

以及设置内参

(6)$$ \begin{cases}
f_x =f l_x
\\
f_y = f l_y
\end{cases}
$$

将(5)(6)代入(4)得有针孔相机模型

(7)$$ \begin{cases}
u = f_x x'+c_x
\\
v =f_y y' + c_y
\end{cases}
$$

 

1.2 图像倾斜修正

现在面阵相机比较普遍,工业上也使用线阵相机,而有些线阵相机也会按线条进行拼接成一个“面阵”,因此最终都需要呈现矩阵像素点阵。如下图a,理想中的相机传感器上的感光器件应该呈方形(或矩形)分布,然而对于一些生产工艺较差的相机,如下图b,其感光器件可能呈现如下右图的平行四边形排布。当出现图b的情形时,这时呈现的图像也跟着倾斜。如果想了解一些成像原理,可以参考文献[1]。

为了解决上图倾斜的问题,不失一般性,以上图b为原型,建立如下图的模型。

这个模型中约定真实成像点应该在\((x_r,y_r)\),因为感光器原因,反馈了一个错误的位置\((x_i,y_i)\),器件沿着\(x\)轴倾斜的角度为\(\theta_x\),沿着\(y\)轴倾斜角度为\(\theta_y\),这时需要得到模型中的坐标\((x_r,y_r)\)。根据模型中的几何关系,可以得到

(8)$$\begin{aligned}&\begin{cases}
\dfrac{l_1}{\sin\theta_y}=\dfrac{y_i}{\sin(\dfrac{\pi}{2}-\dfrac{\theta_y}{2})}
\\
\dfrac{l_1}{\sin \theta_x}=\dfrac{l_2}{\sin\dfrac{\theta_y}{2}}=\dfrac{l_5}{\sin(\pi-\theta_x-\dfrac{\theta_y}{2})}
\\
l_3=\dfrac{x_i-l_5}{\cos \theta_x}
\\
l_4=x_i-l_2-l_3\\
x_r =x_i+l_4\cos\theta_x
\\
y_r = y_i+(l_3+l_4)\sin\theta_x
\end{cases}\\ &\Downarrow
\\&\begin{cases}
x_r =\cos\theta_xx_i+\sin\theta_yy_i
\\
y_r =\sin\theta_xx_i+\cos\theta_y y_i
\end{cases}\end{aligned}
$$

注意,上面公式中\((x_i,y_i)\)与\((x_r,y_r)\)都不是相对相机光心像素点为原点,进一步这里设置\(S\)矩阵

(9)$$S =\begin{bmatrix}
\cos\theta_x & \sin\theta_y
\\
\sin\theta_x & \cos\theta_y
\end{bmatrix}
$$

将(9)代入(8)有

(10)$$ \begin{aligned}
\begin{bmatrix}
x_r
\\
y_r
\end{bmatrix}
&=S\begin{bmatrix}
x_i
\\
y_i
\end{bmatrix}
\\
&=S\begin{bmatrix}
f_xx'+c_{xi}
\\
f_yy'+c_{yi}
\end{bmatrix}
\\
&=S\begin{bmatrix}
f_xx'
\\
f_yy'
\end{bmatrix}+S\begin{bmatrix}
c_{xi}
\\
c_{yi}
\end{bmatrix}
\\
&=S\begin{bmatrix}
f_xx'
\\
f_yy'
\end{bmatrix}+\begin{bmatrix}
c_x
\\
c_y
\end{bmatrix}
\\
&=\begin{bmatrix}
fl_x\cos\theta_x & fl_y\sin\theta_y
\\
fl_x\sin\theta_x & fl_y\cos\theta_y
\end{bmatrix}\begin{bmatrix}
x'
\\
y'
\end{bmatrix}+\begin{bmatrix}
c_x
\\
c_y
\end{bmatrix}
\end{aligned}
$$

公式(10)中的\((c_x,c_y)\)表示修正后的偏移量。进一步,为了简洁表示,这里设置如下变量

(11)$$\begin{cases}
f_x = fl_x\cos\theta_x
\\
f_y=fl_y\cos\theta_y
\\
\alpha_x =fl_x\sin\theta_x
\\
\alpha_y=fl_y\sin\theta_y
\end{cases}$$

设置如下内参矩阵\(K\)

(12)$$K = \begin{bmatrix}
f_x & \alpha_y & c_x
\\
\alpha_x & f_y & c_y
\\
0 & 0 & 1
\end{bmatrix}$$

则可得倾斜修正后的模型

(13)$$ \begin{bmatrix}
u
\\
v
\\
1
\end{bmatrix}=K \begin{bmatrix}
x'
\\
y'
\\
1
\end{bmatrix}$$

注意,内参矩阵\(K\)里的参数\(f_x、f_y、\alpha_x、\alpha_y\)反映了各自缩放与倾斜的信息,特别地,当\(\alpha_x = 0\)时,内参矩阵\(K\)变为如下形式

(14)$$K = \begin{bmatrix}
fl_x & fl_y\sin\theta_y & c_x
\\
0 & fl_y\cos\theta_y & c_y
\\
0 & 0 & 1
\end{bmatrix} = \begin{bmatrix}
f_x & \alpha_y & c_x
\\
0 & f_y & c_y
\\
0 & 0 & 1
\end{bmatrix}=\begin{bmatrix}
f_x & \alpha f_x & c_x
\\
0 & f_y & c_y
\\
0 & 0 & 1
\end{bmatrix}$$

公式(14)中\(\alpha = \dfrac{l_y}{l_x}\sin\theta_y\),OpenCV中就使用的这种表示形式,由于一般\(l_x\)与\(l_y\)值很接近,因此\(\alpha \approx \sin\theta_y\),这时\(\alpha\)主要反映阵列偏离\(y\)轴的程度,这个值越远离0,则越加倾斜

 

PS:

A : 上面导出的修正模型中\(\theta_x、\theta_y\)正负均可,导出的公式一致。

B : 这里的导出方式和一般文献的导出方式正好相反(例如文献[2]),这里基于的原理是给相机一个非矩形正确的光源点位置,但是信号反馈到一个矩形错误的位置,因此对其进行修正,而一般的文献推导是正确的光源点为矩形位置,信号反馈为非矩形位置。

C : 现今的相机,一般\(\theta_x=0、\theta_y=0\),所以很多时候\(K\)少了\(\alpha_x、\alpha_y\)两个内参。

:常规情况下,认为工艺中仅存在左右倾斜的情况,即\(\theta_x=0\),所以大部分文献导出的内参没有\(\alpha_x\)。

E :  常规情况下,\(l_x \approx l_y\),所以大部分情况下会看到\(f_x\)与\(f_y\)接近。

 

1.3 针孔相机畸变

1.2修正的是倾斜,倾斜的模型是线性的,而有时成像扭曲,这时就需要对相机进行畸变处理。

由于相机装配或者本身形状原因,相机图像一般存在畸变,畸变呈现的图形往往扭曲。

1.3.1 径向畸变

由透镜形状引起的畸变称为径向畸变,在透镜拍摄过程中,往往一条直线在相机上显示为一条曲线,在越靠近边缘的地方,这种现象越明显。这里径向畸变主要包括桶形失真(图形上表现为外凸)与枕形失真(图形上表现为内凹)。

由于径向畸变是随着与中心之间的距离增加而增加,因此往往使用一个与径向相关的多项式来进行修正:

(15)$$
\begin{cases}
x_d=x'(1+k_1r'^2+k_2r'^4+k_3r'^6+…)
\\
y_d=y'(1+k_1r'^2+k_2r'^4+k_3r'^6+…)
\end{cases}
$$
公式中\(k_1,k_2,k_3,…\)表示修正系数,一般的相机使用\(k_1,k_2\)修正系数就足够了,而对于鱼眼相机可能还需要加入\(k_3\)作为修正项。这里径向半径 \(r'=\sqrt{x'^2+y'^2}\)。

 

1.3.2 切向畸变
由于相机组装过程中不能使透镜和成像面严格平行,这时会引入切向畸变,对于切向畸变,可使用如下公式修正  
(16)
$$
\begin{cases}
x_d=x'+2p_1x'y'+p_2(r'^2+2x'^2)
\\
y_d=y'+p_1(r'^2+2y'^2)+2p_2x'y'
\end{cases}
$$

1.3.3 针孔相机畸变修正模型
结合(15)(16)得到针孔相机畸变的修正模型  
(17)
$$
\begin{cases}
x_d=x'(1+k_1r'^2+k_2r'^4+k_3r'^6+…)+2p_1x'y'+p_2(r'^2+2x'^2)
\\
y_d=y'(1+k_1r'^2+k_2r'^4+k_3r^6+…)+p_1(r'^2+2y'^2)+2p_2x'y'
\end{cases}
$$

使用修正后的归一化坐标\((x_d,y_d)\)代入(13)的相机模型有

(18)$$
 \begin{bmatrix}
u
\\
v
\\
1
\end{bmatrix}=K \begin{bmatrix}
x_d
\\
y_d
\\
1
\end{bmatrix}
$$

 

1.4 针孔相机内参标定

所谓内参标定,就是通过某种方式确定出内参矩阵\(K\)以及相应的畸变参数。而这里所谓的某种方式,就是在已知输入输出的情况下,反算出内参,这里涉及到两个层面:非线性求解、给定输入输出。

非线性求解,这个需要使用非线性求解器,这里直接采用某种非线性求解算法即可。

给定输入输出,即现实中给定某个特征,这个特征成像后具有某种性质,而根据这种性质建立约束,进而使用非线性方法迭代求解。例如直线特征,现实生活中一条直线,成像后也应该是一条直线,而不是曲线;现实生活中不是特远的矩形块儿,成像后也应该保持矩形特征。所以常见使用方形格子标定板进行标定。当然,也可以不使用方形格子板,自己手动指定一些特征,也是一样的。

还需要注意的是,如果标定时夹杂了位姿(一个变换),如果能提前确定好位姿值最好,否则优化求解中会多位姿变量的求解。这个主要根据构建约束的方式来确定。

 

2 其它相机模型

这里提到的相机模型,均需要去畸变,它们主要不同点是去畸变基于的模型不同。去完畸变后,就简化成一个针孔相机,进而成像。

这里同样记物点在相机坐标系下坐标为\((x,y,z)\),归一化坐标为\((x',y')\),下面的模型主要是对\((x',y')\)修正得到\((x_d,y_d)\),最后代入(18)式。因此每个模型中的内参除了模型需要的几个参数外,还包括\(K\).

 

2.1 UCM模型[3]

UCM模型全称叫Unified Camera Model。这个模型主要用于折反射相机。它是基于一个物点投影到单位球体上,然后再投影到像平面这么一个过程构建模型。这个模型的修正公式如下

(19)$$
\begin{cases}
x_d=\dfrac{x}{\alpha d + (1-\alpha)z}
\\
\\
y_d=\dfrac{y}{\alpha d + (1-\alpha)z}
\end{cases}
$$

公式中\(\dfrac{\alpha}{1-\alpha}\)表示球体中心到像平面距离,而\(d\)则使用如下方式计算

(20)$$d = \sqrt{x^2+y^2+z^2}$$

UCM模型需要的内参为\(K, \alpha\).

 

2.2 EUCM模型[3]

EUCM模型全称叫Extended Unified Camera Model。UCM的镜头是一个圆形球面,为了更具一般性,EUCM将镜头模型扩展到椭圆形,因此增加了一个控制椭圆程度的参数\(\beta\)(此参数越接近1,镜头椭圆度越小,这时镜头面越接近圆球面;否则镜头的椭圆度越大,镜头面就是椭圆面了).

EUCM模型依然使用(19)式进行修正,但\(d\)则使用如下方式计算

(21)$$d = \sqrt{\beta(x^2+y^2)+z^2}$$

EUCM模型需要的内参为\(K, \alpha, \beta\).

 

2.3 KB模型[4]

KB模型全称叫KannalaBrandt Camera Model,它是Juho Kannala和Sami S. Brandt根据几种鱼眼模型(等距投影模型、等立体角投影模型、正交投影模型和体视投影模型)分析得出。KB模型是一个通用模型,适合常见的普通镜头、广角镜头、鱼眼镜头。这个模型的修正公式如下

(22)$$
\begin{cases}
x_d=\dfrac{\theta_d}{r}x
\\
\\
y_d=\dfrac{\theta_d}{r}y
\end{cases}
$$

公式中的\(r, \theta_d\)使用如下公式获取

(23)$$\begin{aligned}
r &= \sqrt{x^2+y^2}
\\
 \theta &=\arctan(r,z)
\\
\theta_d&=\theta+k_1\theta^3+k_2\theta^5+k_3\theta^7+k_4\theta^9+…
\end{aligned}$$

KB模型需要的内参为\(K, k_1, k_2, k_3, k_4, …\).

 

2.4 FOV模型[3]

FOV模型全称叫Field Of View Camera Model。FOV(视场相机)模型, 假设像点与主体点之间的距离近似正比于对应的3D点与光轴之间的角度,因此提出如下修正模型

(24)$$
\begin{cases}
x_d=\dfrac{r_d}{r}x
\\
\\
y_d=\dfrac{r_d}{r}y
\end{cases}
$$

公式中\(r_d\)按如下方式计算

(25)$$r_d=\dfrac{\arctan(2r\tan(w/2), z)}{w}$$

公式中的参数\(w\)表示对应于理想鱼眼镜头的视场。

FOV模型需要的内参为\(K, w\).

 

2.5 DS模型[3]

DS模型全称叫Double Sphere Camera Model,即双球面相机模型。这个模型主体思想是,一个点会被连续投影到两个球体上。这个模型可以看成EUCM的扩展。作者说这个模型更加适合鱼眼相机。这里模型的修正公式如下

(26)$$
\begin{cases}
x_d=\dfrac{x}{\alpha d_2+(1-\alpha)(\xi d_1+z)}
\\
\\
y_d=\dfrac{y}{\alpha d_2+(1-\alpha)(\xi d_1+z)}
\end{cases}
$$

公式中\(d_1、d_2\)按如下方式计算

(27)$$\begin{aligned}d_1 &= \sqrt{x^2+y^2 + z^2}
\\
d_2&=\sqrt{x^2+y^2+(\xi d_1 + z)^2}\end{aligned}$$

公式中\(\xi\)反映了两个单位球体中心的偏移量。

因此DS模型需要的内参为\(K, \alpha, \xi\).

 

参考

[1] 吴建明全面详细解析CMOS和CCD图像传感器

[2] 吕泽乾. 数字图像相关中相机标定技术及其应用拓展[D]. 2020, 中国科学技术大学.

[3] Usenko V , Demmel N , Cremers D . The Double Sphere Camera Model: IEEE, 10.1109/3DV.2018.00069[P]. 2018.

[4] Kannala J , Brandt S S . A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses[J]. IEEE Transactions on Pattern Analysis & Machine Intelligence, 2006, 28(8):1335.

二维平面的旋转表示与性质

1 定义
有时,我们需要在二维平面上处理旋转与平移问题。因此,为了方便计算,这里考虑二维平面上的表示。
在表示之前,这里对一个2维向量\(\mathbf{p}\) ,定义如下运算式  
(1)
$$
\mathbf{p}^\wedge=\begin{bmatrix}
p_x
\\
p_y
\end{bmatrix}^\wedge
=\begin{bmatrix}
0 & 1
\\
-1 & 0
\end{bmatrix} \mathbf{p}
=\begin{bmatrix}
p_y
\\
-p_x
\end{bmatrix}
$$
根据(1)的定义,可以得到如下结论  
(2)
$$
\begin{aligned}
\mathbf{p} &=-\mathbf{p}^{\wedge \wedge} =\mathbf{p}^{\wedge \wedge \wedge \wedge}
\\
(\mathbf{a}^\wedge)^T\mathbf{b}&=\mathbf{b}^T\mathbf{a}^\wedge=-\mathbf{a}^T\mathbf{b}^\wedge
\end{aligned}
$$

1.1 SO(2)
1.1.1 SO(2)定义

在平面坐标系里,只要绕着垂直平面的轴旋转指定角度,就可以得到一个旋转变换,也就是说,可以只用1个旋转角度来表示一个旋转变换。为了统一,这里约定,以右手坐标系里的x-y为平面,设绕z轴逆时针旋转 \(\theta\)角度时,得到的变换为对应为旋转变换。类似SO(3),这里定义如下SO(2)指数映射  
(3)
$$
\begin{aligned}
\exp(\theta) &= R =\begin{bmatrix}
\cos \theta & -\sin\theta
\\
\sin\theta &\cos\theta
\end{bmatrix}
\\
\log(R) &=\theta
\end{aligned}
$$
注意:

① (3)中的 \(\exp、\log\)函数本身只表示一种映射关系,也就是说\(\theta\)与\(R\)一一对应。

② 如果以正北方向为起始绕z轴逆时针旋转\(\theta\),按(3)得到的旋转矩阵表示局部到世界的表示方式,其可以直接用于坐标变化,这时得到的值可直接在一些地图软件上显示。

1.1.2 SO(2)性质
根据(3)中的定义,对于整数\(n\)有如下公式成立  
(4)
$$
\begin{aligned}
\\
\theta &= \arcsin \dfrac{R_{10}-R_{01}}{2}+2n\pi
\\
\exp(-\theta+2n\pi) &= R^T
\\
\exp(\theta_1+\theta_2+2n\pi)&=R_1R_2=R_2R_1
\\
\exp(\theta_1-\theta_2+2n\pi)&=R_1R_2^T=R_2^TR_1
\\
\exp(\theta_2-\theta_1+2n\pi)&=R_1^TR_2=R_2R_1^T
\end{aligned}
$$

1.1.3 SO(2)求导
由于SO(2)本质上只有一个的\(\theta\)变量,从公式(4)可以发现其有很好的性质,因此这里直接采用常规的加法更新求导即可。这里对常见的模型,采用常规加法规则,列举其求导结果如下  
(5) 
$$
\begin{aligned}
\dfrac{\partial R \mathbf{p}}{\partial \theta}&=
\begin{bmatrix}
\sin\theta & -\cos\theta
\\
\cos\theta &\sin\theta
\end{bmatrix}\mathbf{p}\\
&=R^T\begin{bmatrix}
0 & 1
\\
-1 & 0
\end{bmatrix}^T\mathbf{p}
\\
&=R^T(-\mathbf{p})^\wedge
\\
\\
\dfrac{\partial R^T \mathbf{p}}{\partial \theta}&=
\begin{bmatrix}
\sin\theta & \cos\theta
\\
-\cos\theta &\sin\theta
\end{bmatrix}\mathbf{p}
\\
&=R\begin{bmatrix}
0 & 1
\\
-1 & 0
\end{bmatrix}\mathbf{p}
\\
&=R\mathbf{p}^\wedge
\\
\\
\dfrac{\partial \log(R_1R_2R_3)}{\partial \theta_2}&=\dfrac{\theta_1+\theta_2+\theta_3+2n\pi}{\partial\theta_2}
\\&=1
\\
\\
\dfrac{\partial \log(R_1R_2^TR_3)}{\partial \theta_2}&=\dfrac{\theta_1-\theta_2+\theta_3+2n\pi}{\partial\theta_2}
\\&=-1
\end{aligned}
$$

1.2 SE(2)
1.2.1 SE(2)定义
由于SO(2)只能表示一个旋转,为了同时表示旋转\(\theta\)与平移\(\mathbf{t}\),这里使用3维度变量\(\xi\)来表示,即  
(6)
$$
\xi =\begin{bmatrix}
\theta
\\
\mathbf{t}
\end{bmatrix}
$$
注意,上面定义变量时直接使用旋转\(\theta\)与平移\(\mathbf{t}\)来表示,这与SE(3)不同。在SE(3)中,直接使用李代数向量来表示,里面并未直接使用\(\mathbf{t}\),因为\(\mathbf{t}\)里面还包含有旋转部分信息。这里之所以直接使用\(\theta\)与\(\mathbf{t}\),因为下面定义的SE(2)具有很好的性质,在向量与矩阵间转换中直接使用定义的映射就能快速计算。另一方面,在构建边求导过程中,这种定义也不冲突。

对于矩阵形式,参照SE(3)的构建方式,这里定义如下SE(2)映射
(7)
$$
\begin{aligned}
\exp(\xi) &= T = \begin{bmatrix}
R & \mathbf{t}
\\
\mathbf{0}^T &1
\end{bmatrix}
\\
\xi &=\log(T)
\end{aligned}
$$
注意,上面的函数\(\exp\)与\(\log\)仅表示一种映射关系。

1.2.2 SE(2)性质
根据(7)的定义,很容易得到如下性质  
(8)
$$
\begin{aligned}
T^{-1}&=
\exp(\begin{bmatrix}
-\theta+2n\pi
\\
-R^T\mathbf{t}
\end{bmatrix})
\\
T_1T_2&=\exp(\begin{bmatrix}
\theta_1+\theta_2+2n\pi
\\
R_1\mathbf{t}_2+\mathbf{t}_1
\end{bmatrix})
\end{aligned}
$$

1.2.3 SE(2)求导
由于SO(2)使用的常规加法更新求导,且(7)、(8)对加法更新具有很好的适应度,因此这里还是采用常规的加法更新求导模式,对于常见的函数,这里列举如下求导结果  
(9)
$$
\begin{aligned}
\dfrac{\partial (T\mathbf{p}')}{\partial \xi} &=\dfrac{\partial \begin{bmatrix}
R\mathbf{p}+\mathbf{t}
\\
1
\end{bmatrix}}{\partial \xi}
\\
&=\begin{bmatrix}
R\mathbf{p}^\wedge & \mathbf{I}
\\
0 & \mathbf{0}^T
\end{bmatrix}
\\
\\
\dfrac{\partial \log(T_1T_2T_3)}{\partial \xi_2} &=\dfrac{\partial \log(\begin{bmatrix}
R_1R_2R_3 &R_1R_2\mathbf{t}_3+R_1\mathbf{t}_2+\mathbf{t}_1
\\
\mathbf{0}^T & 1
\end{bmatrix})}{\partial \xi_2}
\\
&=\dfrac{\partial \begin{bmatrix}
\theta_1+\theta_2+\theta_3+2n\pi
\\R_1R_2\mathbf{t}_3+R_1\mathbf{t}_2+\mathbf{t}_1
\end{bmatrix}}{\partial \xi_2}
\\
&=\begin{bmatrix}
1 & \mathbf{0}^T
\\
R_1R_2^T(-\mathbf{t}_3)^\wedge & R_1
\end{bmatrix}
\\
\\
\dfrac{\partial \log(T_1T_2^{-1}T_3)}{\partial \xi_2} &=\dfrac{\partial \log(\begin{bmatrix}
R_1R_2^TR_3 &R_1R_2^T(\mathbf{t}_3-\mathbf{t}_2)+\mathbf{t}_1
\\
\mathbf{0}^T & 1
\end{bmatrix})}{\partial \xi_2}
\\
&=\dfrac{\partial \begin{bmatrix}
\theta_1-\theta_2+\theta_3+2n\pi
\\R_1R_2^T(\mathbf{t}_3-\mathbf{t}_2)+\mathbf{t}_1
\end{bmatrix}}{\partial \xi_2}
\\
&=\begin{bmatrix}
-1 & \mathbf{0}^T
\\
R_1R_2(\mathbf{t}_3-\mathbf{t}_2)^\wedge & -R_1R_2^T
\end{bmatrix}
\end{aligned}
$$

IMU预积分

1 背景
在一般的车载传感器上,都会提供IMU惯性测量单元,它主要提供各个时刻的线性加速度(沿着x、y、z的加速度)以及角速度(绕着x、y、z方向的角速度)测量值。由于这些测量值之间与车本身的位姿有一定关系,因此可以通过IMU测量值来估计车本身的位姿。

2 原理
学过初中物理就应该知道,一个物体,已知速度与加速度,这时经过一段时间后,可以计算出各个时间点的速度以及位移值,在整个过程计算中速度、加速度在不断变化,因此求每个时刻的值需要用到积分。由于处理的离散数据,这里的积分可直接累计(累加)的方式进行积分近似。因此所有公式均是基于此推导出来。唯一不同的是,公式里会涉及到旋转矩阵。这里根据文献[1],提供如下的IMU的数学模型  
(1)
$$
\begin{aligned}  
\bar{\omega}_b(t) &= \omega_b(t) + b^\omega(t)+\eta^\omega(t)
\\
\bar{a}_b(t)&=R_{wb}^T(a_w(t)-g_w(t)) + b^a(t)+\eta^a(t)
\end{aligned}
$$
公式中参数含义如下:  
\(\bar{\omega}_b(t)\):表示\(t\)时刻测量到车载坐标系下的角速度。  
\(\omega_b(t)\):表示\(t\)时刻计算到的车载坐标系下的角速度。 
\(b^\omega(t)\):表示\(t\)时刻角速度的偏置,这个参数需要优化。
\(\eta^\omega(t)\):表示\(t\)时刻角速度的白噪声。
\(\bar{a}_b(t)\):表示\(t\)时刻测量到车载坐标系下的线性加速度。  
\(a_w(t)\):表示\(t\)时刻计算到的世界坐标系下的线性加速度。 
\(g_w(t)\):表示\(t\)时刻世界坐标系下的重力加速度。 
\(b^a(t)\):表示\(t\)时刻线性加速度的偏置,这个参数需要优化。
\(\eta^a(t)\):表示\(t\)时刻线性加速度的白噪声。
\(R_{wb}(t)\):表示\(t\)时刻从车载坐标系到世界坐标系的旋转变换。

对于世界坐标系下的速度\(v_w(t)\)以及位置\(p_w(t)\),根据运动方程有  
(2)
$$
\begin{aligned}  
\dot{v}_w(t) &= a_w(t)
\\
\dot{p}_w(t) &=v_w(t)
\\
\dot{R}_{wb}&=R_{wb}\omega_b^\wedge
\end{aligned}
$$
对上面的公式采用欧拉积分有  
(3)
$$
\begin{aligned}  
v_w(t+\Delta t) &= v_w(t) + a_w(t)\Delta t
\\
p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}a_w(t)\Delta t^2
\\
R_{wb}(t+\Delta t)&= R_{wb}(t)\exp(\omega_b(t)\Delta t)
\end{aligned}
$$
将公式(1)代入公式(3)有  
(4)
$$
\begin{aligned}  
v_w(t+\Delta t) &= v_w(t)+g_w(t)\Delta t+R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t
\\
p_w(t+\Delta t)&=p_w(t)+v_w(t)\Delta t+\dfrac{1}{2}g_w(t)\Delta t^2+\dfrac{1}{2}R_{wb}(t)(\bar{a}_b(t)-b^a(t)-\eta^a(t))\Delta t^2
\\
R_{wb}(t+\Delta t)&= R_{wb}(t)\exp((\bar{\omega}_b(t)-b^\omega(t) – \eta^\omega(t))\Delta t)
\end{aligned}
$$
这里记  
(5)
$$
\begin{aligned}  
R_i&=R_{wb}(t)|_{t=i}
\\
p_i&=p_w(t)|_{t=i}
\\
v_i&=v_w(t)|_{t=i}
\\
\bar{\omega}_i&=\bar{\omega}_b(t)|_{t=i}
\\
\omega_i&=\omega_b(t)|_{t=i}
\\
\bar{a}_i&=\bar{a}_b(t)|_{t=i}
\\
a_i&=a_w(t)|_{t=i}
\\
b^\omega_i&=b^\omega(t)|_{t=i}
\\
\eta^\omega_i&=\eta^\omega(t)|_{t=i}
\\
b^a_i&=b^a(t)|_{t=i}
\\
\eta^a_i&=\eta^a(t)|_{t=i}
\\
g&=g_w(t)
\\
\Delta t_{ij}&=\sum_{k=i}^{j-1} \Delta t
\end{aligned}
$$
上面假设重力加速度\(g_w(t)\)为常量。
这里定义如下增量公式  
(6)
$$
\begin{aligned}  
\Delta R_{ij} &=R_i^TR_j
\\
&=\prod_{k=i}^{j-1}\exp((\bar{\omega}_k-b^\omega_k – \eta^\omega_k)\Delta t)
\\
\\
\Delta v_{ij}&=v_j-v_i
\\
&=R_i^T(v_j-v_i-g\Delta t_{ij}) 
\\
&= \sum_{k=i}^{j-1}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t
\\
\\
\Delta p_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t_{ij}^2)
\\
&=\sum_{k=i}^{j-1}[\Delta v_{ik}\Delta t+\dfrac{1}{2}\Delta R_{ik}(\bar{a}_k-b^a_k-\eta^a_k)\Delta t^2]
\end{aligned}
$$
最终根据文献[1]得到如下预积分模型(实际使用的就是如下的模型,即如下约等于符号后的表达式)  
(7)
$$
\begin{aligned}  
\Delta \bar{R}_{ij}&=R_i^TR_j\exp(\delta \phi_{ij})
\\
&\approx R_i^TR_j
\\
\\
\Delta \bar{v}_{ij}&=R_i^T(v_j-v_i-g\Delta t_{ij})+\delta v_{ij}
\\
&\approx R_i^T(v_j-v_i-g\Delta t_{ij})
\\
\\
\Delta \bar{p}_{ij}&=R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})+\delta p_{ij}
\\
&\approx R_i^T(p_j-p_i-v_i\Delta t_{ij}-\dfrac{1}{2}g\Delta t^2_{ij})
\end{aligned}
$$
公式中\(\Delta \bar{R}_{ij}\)、\(\Delta \bar{v}_{ij}\)、\(\Delta \bar{p}_{ij}\)为定义的预积分测量变量,\(\delta R_{ij}\)、\(\delta v_{ij}\)、\(\delta p_{ij}\)为对应白噪声。

如果公式(1)当中的偏置不为常数,则每次需要更新预积分测量变量,这里采用如下公式更新  
(8)
$$
\begin{aligned}  
\Delta \bar{R}_{ij}&\approx \Delta \bar{R}_{ij} \exp (\dfrac{ \partial \Delta \bar{R}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i)
\\
\Delta \bar{v}_{ij}&\approx \Delta \bar{v}_{ij}+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{v}_{ij}}{ \partial b^a_i} \delta b^a_i
\\
\Delta \bar{p}_{ij}&\approx \Delta \bar{p}_{ij}+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^\omega_i} \delta b^\omega_i+\dfrac{ \partial \Delta \bar{p}_{ij}}{ \partial b^a_i} \delta b^a_i
\end{aligned}
$$
上面公式中的偏导数可以参考文献[2].

接下来,我们可以将计算的\(\Delta \bar{R}_{ij}\)、\(\Delta \bar{v}_{ij}\)、\(\Delta \bar{p}_{ij}\)作为测量值,和外部其它变量建立约束。对相关变量进行更新,在实际优化求解中需要优化的变量为\(R_i\)、\(p_i\)、\(v_i\)、\(R_j\)、\(p_j\)、\(v_j\)、\(\delta b^\omega_i\)、\(\delta b^a_i\)。注意变量按如下方式更新  
(9)
$$
\begin{aligned}
R_i &= R_i \exp(\delta \phi_i)
\\
p_i &= p_i + R_i \delta p_i
\\
v_i &= v_i + \delta v_i
\\
R_j &= R_j \exp(\delta \phi_j)
\\
p_j &= p_j + R_j \delta p_j
\\
v_j &= v_j + \delta v_j
\\
\delta b^\omega_i &= \delta b^\omega_i + \bar{\delta b^\omega_i}
\\
\delta b^a_i &= \delta b^a_i + \bar{\delta b^a_i}
\end{aligned}
$$


参考
[1] Forster C ,  Carlone L ,  Dellaert F , et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation (supplementary material)[J]. Georgia Institute of Technology, 2015.

[2] 苏笑云.IMU预积分的理解和推导

[3] 高翔.简明预积分推导
 

坐标系转换之LLA、ECEF、ENU互转

1 名词解释
1.1 ECEF坐标系
也叫地心地固直角坐标系。其原点为地球的质心,x轴延伸通过本初子午线(0度经度)和赤道(0deglatitude)的交点。 z轴延伸通过的北极(即,与地球旋转轴重合)。 y轴完成右手坐标系,穿过赤道和90度经度。

1.2 ENU坐标系
也叫东北天坐标系,坐标系定义为: X轴指向东边,Y轴指向北边,Z轴指向天顶。

1.3 LLA坐标系
也就是也叫经纬高坐标系(经度(longitude),纬度(latitude)和海拔(altitude)LLA坐标系)。也叫做全球地理坐标系、大地坐标系、WGS-84坐标系。

1.4 GCJ02坐标系
GCJ02坐标系,也叫做火星坐标系,是中国国家测绘局制定的一种地理坐标系,它的值与LLA比较接近,LLA与GCJ02坐标系本质上可看成相互之间的微小偏移。

 

2 坐标转换
在经纬高坐标系下,数据并不直观,因此我们更希望将坐标数值转换为常规的直角坐标系(笛卡尔坐标系)下的数据。一种很常见的思想是,假设地球为椭球,然后以地心为原点建立直角坐标系,然后将地球表面的经纬高数据直接转换为对应的直角坐标系数据,也就是常说的LLA转ECEF。虽然这时的ECEF坐标数值已经是直角坐标系下的数值,因为这时的数值很大,受限于程序语言数值计算范围与精度,ECEF的数值在后续数据处理中容易产生数值异常或精度不足的问题。由于我们处理的数据,往往基于地球某一个区域,而这个区域相对于地球来说本身较小,因此这时会在这个区域选择一个锚点,然后以这个锚点为中心建立对应的坐标系,这时以这个区域建立的坐标数值就会小很多,不会带来计算溢出等问题,这时的转换就是ECEF转ENU。ECEF转ENU坐标系本身是直角坐标系下的平移与旋转,因此这种转换本身不破坏数据间的相对关系。

为了后续表示方便,这里选取WGS-84坐标系下经纬度参数,地球基准椭球体的极扁率\(f=\)1/298.257223563,基准椭球体的长半径 \(a=\)6378137.0m,短半径\(b=a\sqrt{1-e^2}\),其中\(e^2=f(2-f)\)。

2.1 LLA转ECEF
这里约定LLA的经度为\(\alpha\),纬度为\(\beta\),海拔为\(h\)。LLA转ECEF的坐标 \((x,y,z)\)为  
(1)
$$
\begin{aligned}
x &= (n+h)\cos \beta \cos \alpha
\\
y &= (n + h)\cos \beta \sin \alpha
\\
z &= (n(1-e^2)+h)\sin \beta
\end{aligned}
$$

上面公式中的\(n\)为如下值  
(2)
$$
n = \dfrac{a}{\sqrt{1-e^2\sin^2\beta}}
$$
2.2 ECEF转LLA
ECEF坐标\((x,y,z)\)转LLA的经度\(\alpha\),纬度\(\beta\),海拔\(h\)。  
ECEF转LLA,本质上是求解公式(1)的非线性方程组,通过观察(1)中的第1与第2个等式,明显可以得到经度的计算表达式,即  
(3)
$$
\alpha = \arctan(y, x)=\arctan(\dfrac{y}{x})
$$
接下来,还剩计算纬度与海拔。计算方法上,本质上还是求解非线性方程组。本文提供三种求解方法。

2.2.1 二分法求解

对(1)进行变形有  
(4)
$$
\begin{aligned}
x^2+y^2 &= (n+h)^2(1-\sin^2 \beta)
\\
z &= (n(1-e^2)+h)\sin \beta
\end{aligned}
$$
为了便于计算,这里设置如下中间量  
(5)
$$
\begin{aligned}
r &=\sqrt{x^2+y^2}
\\
t &= \sin \beta
\end{aligned}
$$
将(5)代入(4)消去\(h\)可以得到
(6)
$$
t=(\dfrac{z}{r}+\dfrac{a}{r}\dfrac{e^2t}{\sqrt{1-e^2t^2}})\sqrt{1-t^2}
$$
这里(6)看似一个不动点迭代式,可惜这个算式并不是对每个值都迭代收敛。进一步,我们设置如下形式  
(7)
$$
f(t)=(\dfrac{z}{r}+\dfrac{a}{r}\dfrac{e^2t}{\sqrt{1-e^2t^2}})\sqrt{1-t^2}-t
$$
从公式(7)我们可以发现,如下等式成立  
(8)
$$
\begin{cases}
f(1)\ \ \ =-1 < 0
\\
f(-1)=1 \ \ \ > 0
\end{cases}
$$
由于\(t\)在[-1,1]区间取值,公式(8)满足使用二分法求解的条件。因此,使用二分法可以求解出满足精度的\(t\)。最终在二分结束后,使用如下方式计算纬度与海拔  
(9)
$$
\begin{aligned}
\beta &=\arcsin(t)
\\
h&=\begin{cases}
\dfrac{r}{\sqrt{1-t^2}}-\dfrac{a}{\sqrt{1-e^2t^2}},\ |t|<0.1
\\
\\
\dfrac{z}{t}-\dfrac{a(1-e^2)}{\sqrt{1-e^2t^2}},\ |t| \geq 0.1
\end{cases}
\end{aligned}
$$

因此,使用二分法求解步骤为:使用(7)进行二分求解出\(t\);使用(9)计算出纬度与海拔;使用(3)计算出经度

二分法在求解出满意精度时,一般需要分割40次。

注意: 在(9)式计算海拔时,为了防止计算除以一个接近0的值,因此以0.1数值作为分段求解。 

2.2.2 不动点迭代求解
二分法虽然有指数级收敛速度,但是依然不够快,这里考虑不动点迭代算法。

为了构建更快的迭代收敛式,这里设置如下中间变量  
(10)
$$
s = (n+h)\sin\beta
$$
将(10)算式代入公式(4)的第1个方程有
(11)
$$
\sin\beta = \dfrac{s}{\sqrt{r^2+s^2}}
$$
将(11)代入公式(4)的第2个方程有  
(12)
$$
s=z+\dfrac{ae^2s}{\sqrt{r^2+s^2-e^2s^2}}
$$
上面表达式即为不动点迭代式,为了证明这个迭代式满足不动点收敛迭代,这里设  
(13)
$$
g(s)=z+\dfrac{ae^2s}{\sqrt{r^2+s^2-e^2s^2}}
$$
对于(13)我们有如下结果  
(14)
$$
\begin{aligned}
g(s_+)&=z+\dfrac{ae^2}{\sqrt{\dfrac{r^2}{s_+^2}+1-e^2}} <z+\dfrac{ae^2}{\sqrt{1-e^2}} \leq |z|+\dfrac{ae^2}{\sqrt{1-e^2}}<|z|+a
\\
g(s_{-}) &=z-\dfrac{ae^2}{\sqrt{\dfrac{r^2}{s_-^2}+1-e^2}} >z-\dfrac{ae^2}{\sqrt{1-e^2}} \geq -|z|-\dfrac{ae^2}{\sqrt{1-e^2}}>-|z|-a
\end{aligned}
$$
因为\(g(s)\)在区间\([-\infty, \infty]\)内的取值在\((-|z|-a, |z|+a)\),说明(12)算式在区间 \([-\infty,\infty]\)存在不动点。

进一步  
(15)
$$
\begin{aligned}
g'(s)&=\dfrac{ar^2e^2}{(r^2+s^2-e^2s^2)^{1.5}}
\\
&=\dfrac{ae^2}{\sqrt{r^2+s^2(1-e^2)}}\dfrac{1}{1+(\dfrac{s}{r})^2(1-e^2)}
\\
&<\dfrac{ae^2}{\sqrt{r^2+s^2(1-e^2)}}
\\
&<\dfrac{ae^2}{r} \leq \dfrac{ae^2}{b}=\dfrac{e^2}{\sqrt{1-e^2}}
\\
&<\dfrac{1}{148.88}\end{aligned}
$$
因此有\(0<g'(s)<\dfrac{1}{148.88}\), 结合(14)的结果,说明(12)算式在区间 \([-\infty, \infty]\)存在不动点迭代,且迭代收敛到唯一值。从(15)可以发现迭代收敛速度较快。在实际迭代时,可以取 \(s=0\)或 \(s=z\)作为初值,然后迭代到满意精度。

最终,迭代完成后,可以使用如下方式获取纬度与海拔:  
(16)
$$
\begin{aligned}
\beta &= \arctan(s,r)=\arctan(\dfrac{s}{r})
\\
h&=\begin{cases}
\sqrt{r^2+s^2}-n,\ |\beta|<\dfrac{7\pi}{16}
\\
\\
\dfrac{z}{\sin\beta}-n(1-e^2),\ |\beta|\geq \dfrac{7\pi}{16}
\end{cases}
\end{aligned}
$$

因此,使用不动点迭代法求解步骤为:使用(12)迭代求解出\(s\);使用(16)计算出纬度与海拔;使用(3)计算出经度。

不动点迭代法在求解出满意精度时,一般迭代4-7次。  

2.2.3 直接求解
使用不动点迭代速度已经很快,但如果还想确保每次计算调用时间可控,那就使用直接求解方法。

对公式(12)进行变形,可得到如下结果:  
(17)
$$
(1-e^2)s^4-2z(1-e^2)s^3+(r^2+z^2(1-e^2)-a^2e^4)s^2-2zr^2s+z^2r^2=0
$$

很明显,(17)是一个关于\(s\)的一元四次方程,由于一元四次方程有单独的求根公式,因此使用求根公式可以求出对应的解。

由于(12)式变到(17)式忽略了正负信息,因此对(17)求根,会存在多解的情况,这时可以将解代入(12)式进行验证,只有一个唯一解满足(12)式。因此最终求得\(s\)。由于(12)式有一定计算量,为了快速筛掉不满足条件的解,通过公式(1)与(10)可以发现,\(s\)与\(z\)的正负性完全一致,因此可以通过解的正负快速判断当前解是否一定是非解。

因此,使用直接法求解步骤为:对(17)式直接求解一元四次方程的解\(s\);使用(16)计算出纬度与海拔;使用(3)计算出经度。

由于使用的直接法求解,求解耗时主要在解一元四次方程的根。但本质上,这种方法求解最稳定、速度也很快,最主要是可控。 

2.3 ECEF与ENU互转
ENU坐标,即将地心坐标经过平移旋转到地表指定点,让新坐标系的z轴指向天空,新坐标的y指向北极,新坐标x轴指向东方。ENU本质上需要一个参考锚点(这里称为Anchor点),然后以这个锚点为原点创建的局部坐标系。假设Anchor的LLA坐标为\((\alpha_0,\beta_0,h_0)\),Anchor的ECEF坐标为\((x_0,y_0,z_0)\),一个点p的ECEF坐标为\((x,y,z)\),p基于Anchor的ENU坐标为\((x_{enu},y_{enu},z_{enu})\),则ECEF与ENU互转过程如下  
(18)  
$$
\begin{aligned}
R_\alpha &=\begin{bmatrix}
\cos(\dfrac{\pi}{2}+\alpha_0) &\sin(\dfrac{\pi}{2}+\alpha_0) &0
\\
-\sin(\dfrac{\pi}{2}+\alpha_0) & \cos(\dfrac{\pi}{2}+\alpha_0) & 0
\\
0 &0&1
\end{bmatrix}
\\
\\ R_\beta &=\begin{bmatrix}
1 &0 &0
\\
0 &\cos(\dfrac{\pi}{2}-\beta_0) &\sin(\dfrac{\pi}{2}-\beta_0)
\\
0 &-\sin(\dfrac{\pi}{2}-\beta_0) & \cos(\dfrac{\pi}{2}-\beta_0)
\end{bmatrix}\\ \\
S &=R_\beta R_\alpha=\begin{bmatrix}
-\sin\alpha_0 & \cos \alpha_0 & 0
\\
-\sin \beta_0 \cos \alpha_0 & -\sin\beta_0\sin\alpha_0 &\cos\beta_0
\\
\cos\beta_0\cos\alpha_0 & \cos\beta_0\sin\alpha_0 & \sin\beta_0
\end{bmatrix}
\\
\\
\begin{bmatrix}
x_{enu}
\\
y_{enu}
\\
z_{enu}
\end{bmatrix} &=S\begin{bmatrix}
x-x_0\\
y-y_0\\
z-z_0
\end{bmatrix}
\\
\\
\begin{bmatrix}
x
\\
y
\\
z
\end{bmatrix} &=S^T\begin{bmatrix}
x_{enu}\\
y_{enu}\\
z_{enu}
\end{bmatrix}+\begin{bmatrix}
x_0
\\
y_0
\\
z_0
\end{bmatrix}
\end{aligned}
$$

很明显上面的S矩阵就是一个旋转变换矩阵,

2.4 LLA与ENU互转
LLA与ENU互转,本质上可通过中间的ECEF坐标系进行转换。这里不再赘述。

 

3 不同锚点选择对数据间相对关系的影响

在数据处理过程中,难免会遇到对数据相对关系进行处理,比如两个点的空间距离,两个点在平面上的距离,两条线在空间的夹角,两个点在平面夹角…… 。

由于ECEF转ENU包含3D旋转操作,因此在不同锚点情况下构建的局部ENU坐标系,单独使用某一个轴(仅考虑转换后的x、y、z数据),或者某两个轴的数据(比如仅考虑所谓平面操作),这时的处理会存在差异。比如以锚点A建立坐标系,这时处理这个坐标系下的平面数据(x-y),其本质上来说锚点A坐标系下x与y的数据含有以锚点B坐标系下的(x-y-z)信息,因此如果以锚点B建立坐标系,这时如果也只处理平面数据(x-y), 在相同处理流程下,两套锚点得到的结果有所差异。

而对于3D空间数据,因为ECEF转ENU本质上是一个刚体变换(不带缩放的3D旋转+平移),由于刚体变换不影响原始数据的相对关系,因此不同锚点的选取,并不影响结果。

因此最终有如下结论:

(1) 如果数据处理流程,包含单独针对某个轴的数据(x或y或z)的处理,  不同锚点的选取,对结果有所影响

(2) 如果数据处理流程,包含单独针对平面(x-y或x-z或y-z)的处理,  不同锚点的选取,对结果有所影响

(3) 如果数据处理流程,所有处理均在3D空间进行,不同锚点的选取,对结果没有影响

 

4 坐标转换测试

目前MathSword软件提供了如下函数供坐标系数据转换: CoorECEFtoENU、CoorECEFtoLLA、CoorENUtoECEF、CoorENUtoLLA、CoorLLAtoECEF、CoorLLAtoENU

对极约束-2D点对匹配建立相机间相对测量约束

1 约定
这里约定对于一个3D点位置为\(\mathbf{p}\),其坐标为(\(x,y,z\)),则\(\mathbf{p}\)采用如下齐次坐标表示  
(1-1)
$$
\mathbf{p}'=\begin{bmatrix}
\mathbf{p}
\\
1
\end{bmatrix}=\begin{bmatrix}
x
\\
y
\\
z
\\
1
\end{bmatrix}
$$
对于相机内参\(K\),约定\(K'\)其为如下4\(\times\)4齐次矩阵  
(1-2)
$$
K'=\begin{bmatrix}
K & \mathbf{0}
\\
\mathbf{0}^T & 1
\end{bmatrix}=\begin{bmatrix}
K_{00} & K_{01} & K_{02} & 0
\\
K_{10} & K_{11} & K_{12} & 0
\\
K_{20} & K_{21} & K_{22} & 0
\\
0 & 0 & 0 & 1
\end{bmatrix}
$$
对于相机位姿\(T\),这里约定的表示为世界坐标到相机坐标的变换,其满足如下定义  
(1-3)
$$
T=\begin{bmatrix}
R & \mathbf{t}
\\
\mathbf{0}^T & 1
\end{bmatrix}
$$

对于相机平面上的像素位置(\(u,v\)),深度为\(s\) ,用点 \(\mathbf{g}\)表示,其坐标形式为  
$$
\mathbf{g} = \begin{bmatrix}
u
\\
v
\\
1
\end{bmatrix}
$$

对于元素个数为1的向量,其表示形式与数值等价。


2 理论

2.1 问题
在空间中有个3D点 \(\mathbf{p}\)在相机1(位姿为\(T_1\))的里的像素位置为\(\mathbf{g}_1\)以及深度值为\(s_1\) ,\(\mathbf{p}\)在相机2(位姿为\(T_2\))的里的像素位置为\(\mathbf{g}_2\)以及深度值为\(s_2\)。现在想建立相机1与相机2之间的相对测量信息。

2.2 相对测量
两个相机位姿间的相对测量不应该随着同一个刚体变换而改变,根据(1-3)的约定,这里两个相机的相对测量\(T\)使用如下定义  
(2-1)
$$
T = T_1T_2^{-1}
$$

注意:这里的相对测量是定义在整体刚体变换情况下,相对测量不变的基础上。也就是说,不管怎么移动,他们之间的相对测量值不再改变。

2.3 已知推导
根据2.1的信息有  
(2-2)
$$
\begin{cases}
s_1\begin{bmatrix}
\mathbf{g}_1
\\
1/s_1
\end{bmatrix}=K_1'T_1\mathbf{p}'
\\
\\
s_2\begin{bmatrix}
\mathbf{g}_2
\\
1/s_2
\end{bmatrix}=K_2'T_2\mathbf{p}'
\end{cases}
\Rightarrow
s_1K_1'^{-1}\begin{bmatrix}
\mathbf{g}_1
\\
1/s_1
\end{bmatrix}=T_1T_2^{-1}(s_2K_2'^{-1}\begin{bmatrix}
\mathbf{g}_2
\\
1/s_1
\end{bmatrix})=T(s_2K_2'^{-1}\begin{bmatrix}
\mathbf{g}_2
\\
1/s_1
\end{bmatrix})
$$
展开上面公式有  
(2-3)
$$
s_1K_1^{-1}\mathbf{g}_1=R(s_2K_2^{-1}\mathbf{g}_2)+\mathbf{t}
$$
上面公式中的\(R\)与\(t\)来自\(T\)。进一步,这里设如下中间变量  
(2-4)
$$
\mathbf{a}_1=K_1^{-1}\mathbf{g}_1
\\
\mathbf{a}_2=K_2^{-1}\mathbf{g}_2
$$
将(2-4)带入公式(2-3)有  
(2-5)
$$
\begin{aligned}
s_1\mathbf{a}_1&=s_2R\mathbf{a}_2+\mathbf{t}
\\
\Downarrow
\\
s_1 \mathbf{t}^\wedge\mathbf{a}_1&=s_2 \mathbf{t}^\wedge R \mathbf{a}_2
\\
\Downarrow
\\
s_1\mathbf{a}_1^T\mathbf{t}^\wedge\mathbf{a}_1&=s_2\mathbf{a}_1^T\mathbf{t}^\wedge R \mathbf{a}_2
\end{aligned}
$$
因为\(\mathbf{a}_1^T\mathbf{t}^\wedge\mathbf{a}_1 = 0\),因此代入(2-5)可以得到如下表达式  
(2-6)
$$
\mathbf{a}_1^T\mathbf{t}^\wedge R \mathbf{a}_2 = 0
$$
因此,最终公式(2-6)就是我们导出的2D匹配点间相机间相关测量约束关系

需要特别注意的是,(2-6)公式存在缺陷,如果使用这个约束作为优化的边,需要谨慎

仔细观察(2-6)可以发现,当\(\mathbf{t}=\mathbf{0}\)时,\(R\)取任意值,均满足上面的表达式,很明显,这种满足无穷组特解的模型对优化有一定误导作用。针对这种情况,需要增加有意义的约束,这里设\(\mathbf{t}=\mathbf{0}\),代入公式(2-5)可得到如下形式  
(2-7)
$$
\mathbf{a}_1^\wedge R \mathbf{a}_2=\mathbf{0}
$$
因此,当\(\mathbf{t}=\mathbf{0}\)时,直接使用公式(2-7)作为约束。

2.4 解的限定

虽然(2-6)与(2-7)给出了限定的约束条件,但这时使用的边依然存在多解的情况,而这些多解中有些并不满足实际的条件。在实际使用中,只有深度\(s_1,s_2\)为正数,才符合我们预期,因此可以将这个判定条件,进一步加入约束。这里设置如下变量

(2-8) $$
\begin{aligned}
\mathbf{b} &=\mathbf{a}_1
\\
\mathbf{c} &=R\mathbf{a}_2
\end{aligned} $$

根据公式(2-5),在\(||\mathbf{t}||_2^2 > 0\)时有

(2-9)
$$
s_1\mathbf{b}=s_2\mathbf{c}+\mathbf{t}
$$

求解(2-9)有

(2-10)$$
\begin{cases}
s_1&=\dfrac{t_2c_0-t_0c_2}{b_2c_0-b_0c_2}=\dfrac{t_2c_1-t_1c_2}{b_2c_1-b_1c_2}=\dfrac{t_1c_0-t_0c_1}{b_1c_0-b_0c_1}
\\
s_2&=-\dfrac{b_0t_2-b_2t_0}{b_0c_2-b_2c_0}=-\dfrac{b_1t_2-b_2t_1}{b_1c_2-b_2c_1}=-\dfrac{b_0t_1-b_1t_0}{b_0c_1-b_1c_0}>0
\end{cases} $$

根据(2-10)可得到如下等效判别式

(2-11)

$$\begin{cases}
(t_2c_0-t_0c_2)(b_2c_0-b_0c_2)>0
\\
(t_2c_1-t_1c_2)(b_2c_1-b_1c_2)>0
\\
(t_1c_0-t_0c_1)(b_1c_0-b_0c_1)>0
\\
(b_0t_2-b_2t_0)(b_0c_2-b_2c_0)<0
\\
(b_1t_2-b_2t_1)(b_1c_2-b_2c_1)<0
\\
(b_0t_1-b_1t_0)(b_0c_1-b_1c_0)<0
\end{cases}$$

对于\(||\mathbf{t}||_2^2 = 0\)时,这时公式(2-5)变成

(2-12)

$$
\begin{aligned}
\dfrac{s_1}{s_2}\mathbf{b}&=\mathbf{c}
\\
&\Downarrow
\\
\dfrac{s_1}{s_2}&=\dfrac{c_0}{b_0}=\dfrac{c_1}{b_1}=\dfrac{c_2}{b_2}>0
\end{aligned}
$$

根据(2-12)可得到如下等效判别式

(2-13)$$\begin{cases}
b_0c_0>0
\\
b_1c_1>0
\\
b_2c_2>0
\end{cases}$$

因此,当使用(2-6)算式约束时,使用(2-11)作为辅助判别约束;当使用(2-7)算式约束时,使用(2-13)作为辅助判别约束。