由上图,可以看到空间三维点P由三角形相似,可以简单计算出来像素平面坐标(u,v)。实际投影成像过程不想上图那么简单,主要包含三步:
[
X
C
Y
C
Z
C
]
⟹
归一化平面
[
u
=
X
C
/
Z
C
v
=
Y
C
/
Z
C
]
⟹
加畸变
[
r
=
u
2
+
v
2
d
r
=
1
+
k
1
r
+
k
2
r
2
+
k
3
r
2
u
d
=
u
d
r
+
2
u
v
p
1
+
(
r
+
2
u
2
)
p
2
v
d
=
v
d
r
+
2
u
v
p
2
+
(
r
+
2
v
2
)
p
1
]
⟹
像素
[
p
x
=
f
x
u
d
+
c
x
p
y
=
f
y
v
d
+
c
y
]
\begin{bmatrix} X_{C}\\Y_{C}\\ Z_{C}\end{bmatrix}\overset{归一化平面}{\Longrightarrow } \begin{bmatrix} u=X_{C}/Z_{C}\\v=Y_{C}/Z_{C}\end{bmatrix}\overset{加畸变}{\Longrightarrow } \begin{bmatrix} r=u^{2}+v^{2}\\d_{r}=1+k_{1}r+k_{2}r^{2}+k_{3}r^{2}\\u_{d}=ud_{r}+2uvp_{1}+(r+2u^{2})p_{2}\\ v_{d}=vd_{r}+2uvp_{2}+(r+2v^{2})p_{1}\end{bmatrix}\overset{像素}{\Longrightarrow }\begin{bmatrix} p_{x}=f_{x}u_{d}+c_{x}\\p_{y}=f_{y}v_{d}+c_{y} \end{bmatrix}
XCYCZC⟹归一化平面[u=XC/ZCv=YC/ZC]⟹加畸变r=u2+v2dr=1+k1r+k2r2+k3r2ud=udr+2uvp1+(r+2u2)p2vd=vdr+2uvp2+(r+2v2)p1⟹像素[px=fxud+cxpy=fyvd+cy]
上图第二步以rantan畸变为例,对于equidistant畸变模型则是另外一种形式:
{
r
=
u
2
+
v
2
θ
d
=
θ
(
1
+
k
1
θ
2
+
k
2
θ
4
+
k
3
θ
6
+
k
4
θ
8
)
u
d
=
u
⋅
θ
d
r
,
v
d
=
v
⋅
θ
d
r
\left\{\begin{matrix} r=\sqrt{u^2+v^2} \\ \theta_{d}=\theta(1+k_{1}\theta^{2}+k_{2}\theta^{4}+k_{3}\theta^{6}+k_{4}\theta^{8}) \\ u_{d}=u\cdot \frac{\theta_{d}}{r},v_{d}=v\cdot \frac{\theta_{d}}{r} \end{matrix}\right.
⎩⎨⎧r=u2+v2θd=θ(1+k1θ2+k2θ4+k3θ6+k4θ8)ud=u⋅rθd,vd=v⋅rθd
用 T 来表示不同坐标系之间的变换,在相机和 IMU 使用前,需要对相机和 IMU 之间的变换矩阵
T
c
b
T_{cb}
Tcb进行标定。因此相机坐标系和 IMU 坐标系之间满足如下变换关系:
T
w
b
=
T
w
c
⋅
T
c
b
T_{wb}=T_{wc}\cdot T_{cb}
Twb=Twc⋅Tcb
将 T 表示成旋转矩阵 R 和平移向量 t:
将上式进行矩阵之间的运算,可以分别得到相机坐标系和 IMU 坐标系之间旋转和平移的变换关系:
由于相机和 IMU 是两个不同的传感器,两者的触发不能完全同步进行,因此在使用中相机和 IMU 之间触发存在时间差
t
d
t_{d}
td。如下图 所示,传感器采样实例和时间戳之间存在偏差。
传感器采样时间戳
IMU 的采样时间等于相机的采样时间加上时间差
t
d
t_{d}
td:
t
I
M
U
=
t
c
a
m
+
t
d
t_{IMU}=t_{cam}+t_{d}
tIMU=tcam+td
三.Camera-IMU标定模型
对于低纹理、运动模糊和遮挡的环境,视觉和惯性传感器的结合具有较强的鲁棒性。在纯视觉 SLAM 中通常缺少尺度信息,IMU 通过提供可靠的 R 和 t 以恢复绝对尺度。具有代表性的算法有 EKF-SLAM、MSCKF、OKVIS、VINS-Mono和ORB-SLAM3。以上经典的算法都依赖于系统准确的初始化和相机-IMU精准的标定。相机-IMU 的外参标定主要来标定从相机坐标系{C}到 IMU 坐标系{B}的变换矩阵(即旋转和平移),我们首先求取从相机到 IMU 的旋转,然后再求取平移.
(一) 相机-IMU旋转
相机-IMU 之间的旋转对于视觉惯性 SLAM 系统的鲁棒性非常重要,过大的偏差会导致系统的初始化崩溃。因为单目相机可以跟踪系统的位姿,而两幅图像之间的相对旋转可以通过经典的五点算法来解决两帧之间图片的相对旋转
R
c
k
,
c
k
+
1
R_{c_{k},c_{k+1}}
Rck,ck+1。此外,角速度可以通过对陀螺仪积分来获得相关的旋转
R
b
k
,
b
k
+
1
R_{b_{k},b_{k+1}}
Rbk,bk+1。对于任意的 k 帧的图片,遵循以下等式:
R
b
k
,
b
k
+
1
⋅
R
b
c
=
R
b
c
⋅
R
c
k
,
c
k
+
1
R_{b_{k},b_{k+1}}\cdot R_{bc}=R_{bc}\cdot R_{c_{k},c_{k+1}}
Rbk,bk+1⋅Rbc=Rbc⋅Rck,ck+1
将上式中的旋转矩阵用四元数来表示:
对于给定的多对连续图像之间的旋转,我们可以构建一个超定方程:
式中 N 表示旋转矩阵收敛时所使用帧的数量,
w
k
,
k
+
1
w_{k,k+1}
wk,k+1是异常处理的权重。当旋转校准与传入的测量一起运行时,先前估计的结果
R
^
b
c
\hat{R}_{bc}
R^bc可以作为初始值对残差进行加权:
残差函数的权重为:
如果没有足够的特征来估计相机旋转,则将
w
k
,
k
+
1
w_{k,k+1}
wk,k+1设置为零。以上的超定方程的解可以找到对应于
Q
N
Q_{N}
QN的最小奇异值的右单位奇异向量。即可以解算出对应的旋转矩阵
R
b
c
R_{bc}
Rbc。
(二) 相机-IMU平移
相机-IMU 的旋转可以从上部分得到,获得旋转矩阵
R
b
c
R_{bc}
Rbc后,我们可以估计相机-IMU 的平移,使用紧耦合的滑动窗口来初始化这些参数。初始化在 IMU 坐标系下完成,我们定义的状态向量为:
其中
x
k
x_{k}
xk是第 k 个 IMU 的状态,
g
b
k
g^{bk}
gbk是重力向量,N 是 IMU 的状态在滑动窗口中 s 的数量,M是在滑动窗口中具有足够视差的特征的数量。n 和 m 是在滑动窗口中的起始索引。
λ
l
λ_{l}
λl 是第
l
l
l个特征从第一次观察开始的深度,
P
b
0
b
k
P_{b_{0}b_{k}}
Pb0bk=[0,0,0]事前设定好。
其中 B 是所有 IMU 测量值的集合,C 是任何特征和任何相机姿态之间的所有观测值的集合。由于增量和相对旋转是已知的,上式可以使用非迭代线性方式求解。其中 IMU 的测量可以表示为:
{
r
p
,
H
p
r_{p},H_{p}
rp,Hp}是线性估计器的解,{
P
b
k
,
b
k
+
1
,
H
b
k
,
b
k
+
1
P_{b_{k},b_{k+1}},H_{b_{k},b_{k+1}}
Pbk,bk+1,Hbk,bk+1}是线性 IMU 测量模型,
H
c
j
l
H_{cjl}
Hcjl是线性相机测量模型。上式最优化方程描述的过于复杂,为了便于参数的求解,将线性代价函数转换为以下形式:
其中{$ \Lambda_{B},b_{B}KaTeX parse error: Expected 'EOF', got '}' at position 1: }̲和{\Lambda_{C},b_{C}$}分别是 IMU 和视觉测量的信息矩阵和向量。由于已知的增量和对应的旋转,代价函数的状态是线性的,并且上式具有唯一的解。通过对该方程线性的求解,就能获得相机-IMU 之间的平移向量
t
b
c
t_{bc}
tbc。
(三) 视觉惯性代价函数
单目视觉惯性问题可以被公式化为一个联合优化的代价函数
J
(
x
)
J(x)
J(x),代价函数包括视觉测量的残差权重
e
v
e_{v}
ev和惯性测量的残差权重
e
i
e_{i}
ei:
其中 i 是图像的特征索引,k 代表相机数量索引,j 表示 3D 目标的位置,W 表示位置测量的信息矩阵。
这里对pose参数化采用六维的列向量,分别三维的位移和旋转矢量 。对位移和旋转矢量分别求一阶导、二阶导可以得到速度与加速度:
v
=
t
˙
v = \dot{t}
v=t˙
a
=
t
˙
a = \dot{t}
a=t˙
w
i
=
J
r
(
θ
)
θ
˙
w_{i} = J_{r}(\theta)\dot{\theta}
wi=Jr(θ)θ˙
利用camera的样条曲线获取任意时刻camera旋转角速度,而陀螺仪又测量imu的角速度。忽略偏置和噪声影响,两者相差一个旋转,且模长相等:
w
i
=
R
i
c
w
c
w_{i}=R_{ic}w_{c}
wi=Ricwc
(二) 获取imu-camera之间初始旋转,还有一些必要的初始值:重力加速度、陀螺仪偏置
同样利用角速度测量关系,这次构造一个优化问题
这样就可以获得camera-imu之间的旋转,以及陀螺仪偏置初始值。
忽略加速度偏置与噪声,假设整个标定过程中平均加速度为零,所以也可以获得重力加速度在参考坐标系下的表示:
g
^
=
−
1
n
∑
i
=
1
n
R
w
c
∗
R
c
i
∗
a
m
\hat{g} =-\frac{1}{n} \sum_{i=1}^{n} R_{wc}*R_{ci}*a_{m}
g^=−n1∑i=1nRwc∗Rci∗am