視覺SLAM十四講|【五】相機(jī)與IMU時(shí)間戳同步
相機(jī)成像方程
Z
[
u
v
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
Y
Z
]
=
K
P
Z \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}= KP
Z
?uv1?
?=
?fx?00?0fy?0?cx?cy?1?
?
?XYZ?
?=KP
其中，
K
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
K=\begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}
K=
?fx?00?0fy?0?cx?cy?1?
?
時(shí)間戳同步
假設(shè)視覺特征在圖像平面上勻速移動，則特征在相機(jī)成像平面上的運(yùn)動速度為
V
l
k
=
(
[
u
l
k
+
1
v
l
k
+
1
]
?
[
u
l
k
v
l
k
]
)
/
(
t
k
+
1
?
t
k
)
V_l^k =( \begin{bmatrix} u_l^{k+1} \\ v_l^{k+1} \end{bmatrix} - \begin{bmatrix} u_l^{k} \\ v_l^{k} \end{bmatrix})/(t_{k+1}-t_k)
Vlk?=([ulk+1?vlk+1??]?[ulk?vlk??])/(tk+1??tk?)
設(shè)世界坐標(biāo)系中
l
l
l個(gè)地圖點(diǎn)坐標(biāo)為
f
l
w
=
[
x
,
y
,
z
]
T
f_l^w = [x, y,z]^T
flw?=[x,y,z]T
變換到相機(jī)坐標(biāo)系下則為
f
l
c
i
=
R
c
b
R
w
b
i
T
(
f
l
w
?
p
w
b
i
)
+
p
c
b
f_l^{c_i} = R_{cb}R_{wb_i}^T(f_l^w - p_{wb_i}) + p_{cb}
flci??=Rcb?Rwbi?T?(flw??pwbi??)+pcb?再投影到圖像平面，并計(jì)算重投影殘差
r
c
=
[
x
l
i
z
l
i
?
u
l
i
,
y
l
i
z
l
i
?
v
l
i
]
T
r_c = [\frac{x_l^i}{z_l^i}-u_l^i, \frac{y_l^i}{z_l^i}-v_l^i]^T
rc?=[zli?xli???uli?,zli?yli???vli?]T
考慮時(shí)間延遲對特征坐標(biāo)的補(bǔ)償為
z
l
i
(
t
d
)
=
[
u
l
i
,
v
l
i
]
T
+
t
d
v
l
i
z_l^i(t_d) = [u_l^i, v_l^i]^T + t_d v_l^i
zli?(td?)=[uli?,vli?]T+td?vli?文章來源：http://www.zghlxwxcb.cn/news/detail-808079.html
逆深度參數(shù)化方式
SLAM中特征點(diǎn)的參數(shù)化表示有很多，最直接的是用三維坐標(biāo)XYZ來表示，但通常大家更喜歡用逆深度表示，因?yàn)槟嫔疃葍?yōu)勢在于能夠建模無窮遠(yuǎn)點(diǎn)?；仡櫹鄼C(jī)成像方程
Z
[
u
v
1
]
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
[
X
Y
Z
]
=
K
P
Z \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}= \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}= KP
Z
?uv1?
?=
?fx?00?0fy?0?cx?cy?1?
?
?XYZ?
?=KP
有
P
=
1
λ
K
?
1
[
u
v
1
]
P=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u \\v \\ 1 \end{bmatrix}
P=λ1?K?1
?uv1?
?
對于世界坐標(biāo)系中的某相機(jī)觀測點(diǎn)
f
l
c
i
f_l^{c_i}
flci??，可以用相機(jī)逆深度成像公式得到，如下所示
f
l
c
i
=
1
λ
K
?
1
[
u
l
i
v
l
i
1
]
f_l^{c_i}=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix}
flci??=λ1?K?1
?uli?vli?1?
?
考慮到坐標(biāo)系轉(zhuǎn)換關(guān)系
f
l
w
=
R
w
c
i
f
l
c
i
+
p
w
c
i
f_l^w = R_{wc_i}f_l^{c_i}+p_{wc_i}
flw?=Rwci??flci??+pwci??
觀測點(diǎn)
f
l
c
i
f_l^{c_i}
flci??也可以通過運(yùn)動姿態(tài)進(jìn)行推測，有
f
l
c
i
~
=
R
c
i
w
f
l
w
+
p
c
i
w
\tilde{{f_l^{c_i}}} = R_{c_iw}f_l^w+p_{c_iw}
flci??~?=Rci?w?flw?+pci?w?
我們研究的是投影面內(nèi)的殘差，因此，不考慮時(shí)間延遲的殘差可以寫為如下形式
r
c
3
=
[
u
l
j
v
l
j
1
]
?
λ
K
f
l
c
j
~
r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K \tilde{{f_l^{c_j}}}
rc3?=
?ulj?vlj?1?
??λKflcj??~?
r
c
3
=
[
u
l
j
v
l
j
1
]
?
λ
K
(
R
c
j
w
f
l
w
+
p
c
j
w
)
r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}f_l^w + p_{c_jw})
rc3?=
?ulj?vlj?1?
??λK(Rcj?w?flw?+pcj?w?)
r
c
3
=
[
u
l
j
v
l
j
1
]
?
λ
K
(
R
c
j
w
(
R
w
c
i
f
l
c
i
+
p
w
c
i
)
+
p
c
j
w
)
r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}(R_{w c_i}f_l^{c_i}+p_{wc_i})+ p_{c_jw})
rc3?=
?ulj?vlj?1?
??λK(Rcj?w?(Rwci??flci??+pwci??)+pcj?w?)
又因?yàn)?br>
f
l
c
i
=
1
λ
K
?
1
[
u
l
i
v
l
i
1
]
f_l^{c_i}=\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix}
flci??=λ1?K?1
?uli?vli?1?
?
r
c
3
=
[
u
l
j
v
l
j
1
]
?
λ
K
(
R
c
j
w
(
R
w
c
i
(
1
λ
K
?
1
[
u
l
i
v
l
i
1
]
)
+
p
w
c
i
)
+
p
c
j
w
)
r_{c3} = \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda K(R_{c_jw}(R_{w c_i}(\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix})+p_{wc_i})+ p_{c_jw})
rc3?=
?ulj?vlj?1?
??λK(Rcj?w?(Rwci??(λ1?K?1
?uli?vli?1?
?)+pwci??)+pcj?w?)
[
u
l
j
v
l
j
]
=
[
1
0
0
0
1
0
]
[
u
l
j
v
l
j
1
]
\begin{bmatrix} u_l^j\\ v_l^j \end{bmatrix} = \begin{bmatrix} 1 & 0 &0 \\ 0 & 1 & 0 \end{bmatrix} \begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix}
[ulj?vlj??]=[10?01?00?]
?ulj?vlj?1?
?
令
C
=
[
1
0
0
0
1
0
]
C = \begin{bmatrix} 1 & 0 &0 \\ 0 & 1 & 0 \end{bmatrix}
C=[10?01?00?]
有
r
c
=
C
r
c
3
r_c = C r_{c3}
rc?=Crc3?
r
c
=
C
[
u
l
j
v
l
j
1
]
?
λ
C
K
(
R
c
j
w
(
R
w
c
i
(
1
λ
K
?
1
[
u
l
i
v
l
i
1
]
)
+
p
w
c
i
)
+
p
c
j
w
)
r_{c} = C\begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} - \lambda CK(R_{c_jw}(R_{w c_i}(\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix})+p_{wc_i})+ p_{c_jw})
rc?=C
?ulj?vlj?1?
??λCK(Rcj?w?(Rwci??(λ1?K?1
?uli?vli?1?
?)+pwci??)+pcj?w?)
現(xiàn)在考慮時(shí)間延遲
r
c
=
C
(
[
u
l
j
v
l
j
1
]
+
v
j
t
d
)
?
λ
C
K
(
R
c
j
w
(
R
w
c
i
(
1
λ
K
?
1
[
u
l
i
v
l
i
1
]
)
+
p
w
c
i
)
+
p
c
j
w
)
r_{c} = C(\begin{bmatrix} u_l^j\\ v_l^j \\1 \end{bmatrix} + v_jt_d) - \lambda CK(R_{c_jw}(R_{w c_i}(\frac{1}{\lambda}K^{-1} \begin{bmatrix}u_l^i \\v_l^i \\ 1 \end{bmatrix})+p_{wc_i})+ p_{c_jw})
rc?=C(
?ulj?vlj?1?
?+vj?td?)?λCK(Rcj?w?(Rwci??(λ1?K?1
?uli?vli?1?
?)+pwci??)+pcj?w?)
其中
v
j
=
(
[
u
k
+
1
v
k
+
1
1
]
?
[
u
k
v
k
1
]
)
/
(
t
k
+
1
?
t
k
)
v_j =(\begin{bmatrix} u_{k+1} \\ v_{k+1} \\ 1\end{bmatrix} - \begin{bmatrix} u_{k} \\ v_{k} \\ 1\end{bmatrix})/(t_{k+1}-t_k)
vj?=(
?uk+1?vk+1?1?
??
?uk?vk?1?
?)/(tk+1??tk?)文章來源地址http://www.zghlxwxcb.cn/news/detail-808079.html
到了這里，關(guān)于視覺SLAM十四講|【五】相機(jī)與IMU時(shí)間戳同步的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
