在上一篇中已经提到,我们打算把激光雷达坐标系到车体中心坐标系之间的外参进行在线估计。假如激光雷达的外参标定精度较差,对EKF系统会有影响,此外,假如在车体运动过程中,由于抖动等造成激光雷达安装位置发生了变化,则在线估计在所难免。
在以下推导中,我们不再区分建图和定位过程,都统一认为:老反光板地图可有可无,状态空间中维护新的反光板位置。
在线外参估计EKF公式推导
状态空间至少包含6个自由度:机器人的实时位姿 x , y , θ x,y,\theta x,y,θ,和激光雷达坐标系到车体坐标系的转换: l b x , l b y , l b θ ^b_lx,^b_ly,^b_l\theta lbx,lby,lbθ
状态空间
假如当前的状态空间中有 N N N个新增的反光板( N > = 0 N>=0 N>=0),此时,状态空间为:
X = [ x y θ l b x l b y l b θ m x 1 m y 1 . . . m x N m y N ] T X=\begin{bmatrix}x & y &\theta &^b_lx&^b_ly&^b_l\theta & m_x^1 & m_y^1 &...& m_x^N & m_y^N\end{bmatrix}^T X=[xyθlbxlbylbθmx1my1...mxNmyN]T
运动模型-预测
(1)位姿预测:
初始位姿 x ˇ , y ˇ , θ ˇ \check x, \check y,\check \theta xˇ,yˇ,θˇ,标定初始值为 l b x ˇ , l b y ˇ , l b θ ˇ ^b_l\check{x}, ^b_l\check{y}, ^b_l\check{\theta} lbxˇ,lbyˇ,lbθˇ ,初始位姿的协方差,手动给定:
Σ ξ 0 = [ Σ x 0 0 0 Σ y 0 0 0 Σ θ ] 3 ∗ 3 \Sigma_{\xi0} = \begin{bmatrix}\Sigma_{x} & 0 & 0 \\ 0 & \Sigma_{y} & 0 \\ 0 & 0 & \Sigma_{\theta}\end{bmatrix}_{3*3} Σξ0=⎣⎡Σx000Σy000Σθ⎦⎤3∗3
标定初始值的协方差,手动给定:
Σ i n i t = [ Σ x 0 0 0 Σ y 0 0 0 Σ θ ] 3 ∗ 3 \Sigma_{init} = \begin{bmatrix}\Sigma_{x}&0&0\\0&\Sigma_{y}&0\\0&0&\Sigma_{\theta}\end{bmatrix}_{3*3} Σinit=⎣⎡Σx000Σy000Σθ⎦⎤3∗3
运动方程可以写为:
[ x t y t θ t l b x t l b y t l b θ t m x 1 m y 1 . . . m x N m y N ] 2 ∗ N + 6 = [ x t − 1 y t − 1 θ t − 1 l b x t − 1 l b y t − 1 l b θ t − 1 m x 1 m y 1 . . . m x N m y N ] 2 ∗ N + 6 + [ v t Δ t cos ( θ t − 1 + ω t Δ t / 2 ) v t Δ t sin ( θ t − 1 + ω t Δ t / 2 ) ω t Δ t 0 0 0 0 0 . . . 0 0 ] 2 ∗ N + 6 \begin{bmatrix}x_t \\ y_t \\ \theta_t \\ ^b_lx_t \\^b_ly_t\\^b_l\theta_t\\ m_x^1 \\ m_y^1 \\...\\ m_x^N \\ m_y^N\end{bmatrix}_{2*N+6} = \begin{bmatrix}x_{t-1} \\ y_{t-1} \\ \theta_{t-1 }\\ ^b_lx_{t-1} \\^b_ly_{t-1}\\^b_l\theta_{t-1} \\m_x^1 \\ m_y^1 \\...\\ m_x^N \\ m_y^N\end{bmatrix}_{2*N+6} + \begin{bmatrix}v_t\Delta t\cos(\theta_{t-1}+\omega_t\Delta t / 2) \\ v_t\Delta t\sin(\theta_{t-1}+\omega_t\Delta t / 2) \\ \omega_t\Delta t \\ 0 \\ 0 \\ 0 \\ 0 \\ 0\\...\\0\\0\end{bmatrix}_{2*N+6} ⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xtytθtlbxtlbytlbθtmx1my1...mxNmyN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+6=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡xt−1yt−1θt−1lbxt−1lbyt−1lbθt−1mx1my1...mxNmyN⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+6+⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡vtΔtcos(θt−1+ωtΔt/2)vtΔtsin(θt−1+ωtΔt/2)ωtΔt00000...00⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤2∗N+6
假设轮速计的线速度和角速度服从均值为0的高斯分布,即控制的协方差为 Σ u \Sigma_u Σu
Σ u = [ σ v 2 0 0 σ ω 2 ] 2 ∗ 2 \Sigma_u=\begin{bmatrix}\sigma_{v}^2 & 0 \\ 0 & \sigma_{\omega}^2\end{bmatrix}_{2*2} Σu=[σv200σω2]2∗2
(2)协方差预测
状态量在 t t t时刻的协方差预测方程为: Σ t = G t Σ t − 1 G t T + G u Σ u G u T \Sigma_{t} = G_{t}\Sigma_{t-1}G_{t}^T+G_u\Sigma_uG_u^T Σt=GtΣt−1GtT+GuΣuGuT
其中:
G t G_{t} Gt是运动模型关于状态 X t − 1 X _{t-1} Xt−1的雅克比:
G t = ∂ X t ∂ X t − 1 = [ 1 0 − v t Δ t sin ( θ t − 1 + ω t Δ t / 2 ) 0 0 0 0 0 . . . 0 0 0 1 v t Δ t cos ( θ t − 1 + ω t Δ t / 2 ) 0 0 0 0 0 . . . 0 0 0 0 1 0 0 0 0 0 . . . 0 0 0 0 0 1 0 0 0 0 . . . 0 0 0 0 0 0 1 0 0 0 . . . 0 0 0 0 0 0 0 1 0 0 . . . 0 0 0 0 0 0 0 0 1 0 . . . 0 0 0 0 0 0 0 0 0 1 . . . 0 0 . . . . . . . . . . . . . . . . . . 0 0 0 0 0 0 0 0 . . . 1 0 0 0 0 0 0 0 0 0 . . . 0 1 ] ( 2 ∗ N + 6 ) ∗ ( 2 ∗ N + 6 ) G_{t} = \frac{\partial X_t}{\partial X_{t-1}}=\begin{bmatrix}1 & 0 & -v_t\Delta t\sin(\theta_{t-1}+\omega_t\Delta t / 2) & 0 & 0&0&0&0&...&0&0 \\ 0 & 1 & v_t\Delta t\cos(\theta_{t-1}+\omega_t\Delta t / 2)& 0 & 0&0&0&0&...&0&0 \\ 0 & 0 & 1 & 0 & 0&0&0&0&...&0&0 \\ 0&0&0 &1 & 0&0 &0&0&...&0&0 \\ 0&0&0&0&1&0&0&0&...&0&0 \\ 0&0&0&0&0&1&0&0&...&0&0\\ 0&0&0&0&0&0&1&0&...&0&0\\0&0&0&0&0&0&0&1&...&0&0\\ ... & ...&...&&&&&&...&...&...\\ 0&0&0&0&0&0&0&0&...&1&0\\0&0&0&0&0&0&0&0&...&0&1\end{bmatrix}_{(2*N+6)*(2*N+6)} Gt=∂Xt−1∂Xt=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡10000000...0001000000...00−vtΔtsin(θt−1+ωtΔt/2)vtΔtcos(θt−1+ωtΔt/2)100000...0000010000000000100000000001000000000010000000000100.................................00000000...1000000000...01⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(2∗N+6)∗(2∗N+6)
G u G_u Gu是运动模型关于控制(轮速计线速度和角速度) u = [ v t ω t ] T u = \begin{bmatrix}v_t & \omega_t\end{bmatrix}^T u=[vtωt]T的雅克比:
G u = ∂ X t ∂ u = [ Δ t cos ( θ t − 1 + ω t Δ t / 2 ) − v t Δ t 2 sin ( θ t − 1 + ω t Δ t / 2 ) 2 Δ t sin ( θ t − 1 + ω t Δ t / 2 ) v t Δ t 2 cos ( θ t − 1 + ω t Δ t / 2 ) 2 0 Δ t 0 0 0 0 0 0 0 0 0 0 . . . . . . 0 0 0 0 ] ( 2 ∗ N + 6 ) ∗ 2 G_u = \frac{\partial X_t}{\partial u}=\begin{bmatrix}\Delta t\cos(\theta_{t-1}+\omega_t\Delta t / 2) & \frac{ -v_t\Delta t^2\sin(\theta_{t-1}+\omega_t\Delta t / 2)}{2} \\ \Delta t\sin(\theta_{t-1}+\omega_t\Delta t / 2) & \frac{ v_t\Delta t^2\cos(\theta_{t-1}+\omega_t\Delta t / 2)}{2} \\ 0 & \Delta t \\ 0&0\\ 0&0\\ 0&0\\ 0&0 \\ 0&0\\ ...& ...\\ 0&0\\0&0\end{bmatrix}_{(2*N+6)*2} Gu=∂u∂Xt=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡Δtcos(θt−1+ωtΔt/2)Δtsin(θt−1+ωtΔt/2)000000...002−vtΔt2sin(θt−1+ωtΔt/2)2vtΔt2cos(θt−1+ωtΔt/2)Δt00000...00⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤(2∗N+6)∗2
观测模型
假设当前激光帧中检测到 K K K个反光板 [ r x 1 r y 1 . . . r x i r y i . . . r x K r y K ] 2 K ∗ 1 \begin{bmatrix}r_x^1 & r_y^1 & ...&r_x^i& r_y^i&... &r_x^K & r_y^K\end{bmatrix}_{2K*1} [rx1ry1...rxiryi...rxKryK]2K∗1,需要分为三种情况讨论:
(1)通过数据关联(欧式距离或马氏距离等手段),我们找到老地图中对应的 k 1 k1 k1个反光板;
(2)通过数据关联,我们找到状态空间中对应的 k 2 k2 k2个反光板;
(3)无法关联上的反光板有 k 3 = K − k 1 − k 2 k3=K-k1-k2 k3=K−k1−k2个。
对于关联不上的反光板,我们不放进观测方程中,在后续进行状态空间的扩展。
车体到世界坐标系下的转换为: b W T = [ R 1 t 1 0 1 ] ^W_bT=\begin{bmatrix}R_1&t_1\\0&1\end{bmatrix} bWT=[R10t11], R 1 = [ cos θ t − sin θ t sin θ t cos θ t ] , t 1 = [ x t y t ] R_1=\begin{bmatrix}\cos\theta_t&-\sin\theta_t\\\sin\theta_t & \cos\theta_t\end{bmatrix},t_1 =\begin{bmatrix}x_t\\y_t\end{bmatrix} R1=[cosθtsinθt−sinθtcosθt],t1=[xtyt]
激光雷达坐标系到车体坐标系的转换为: l b T = [ R 2 t 2 0 1 ] ^b_lT=\begin{bmatrix}R_2&t_2\\0&1\end{bmatrix} lbT=[R20t21], R 2 = [ cos l b θ t − sin l b θ t sin l b θ t cos l b θ t ] , t 2 = [ l b x t l b y t ] R_2=\begin{bmatrix}\cos^b_l\theta_t&-\sin^b_l\theta_t\\\sin^b_l\theta_t & \cos^b_l\theta_t\end{bmatrix},t_2 = \begin{bmatrix}^b_lx_t\\^b_ly_t\end{bmatrix} R2=[coslbθtsinlbθt−sinlbθtcoslbθt],t2=[lbxtlbyt]
激光雷达坐标系到世界坐标系的转换为: l W T = b W T ∗ l b T = [ R t 0 1 ] ^W_lT = ^W_bT*^b_lT =\begin{bmatrix}R&t\\0&1\end{bmatrix} lWT=bWT∗lbT=[R0t1]
R = R 1 ∗ R 2 = [ cos ( θ t + l b θ t ) − sin ( θ t + l b θ t ) sin ( θ t + l b θ t ) cos ( θ t + l b θ t ) ] R = R1 * R2 =\begin{bmatrix}\cos(\theta_t + ^b_l\theta_t)&-\sin(\theta_t + ^b_l\theta_t)\\\sin(\theta_t + ^b_l\theta_t) & \cos(\theta_t + ^b_l\theta_t)\end{bmatrix} R=R1∗R2=[cos(θt+lbθt)sin(θt+lbθt)−sin(θt+lbθt)cos(θt+lbθt)]
t = R 1 ∗ t 2 + t 1 t = R_1 * t_2 + t_1 t=R1∗t2+t1
即有: W P = l W T ∗ l P ^WP = ^W_lT*^lP WP=lWT∗lP
而在观测方程中,我们需要将激光雷达的观测放在左边,即:
l
P
=
W
l
T
∗
W
P
^lP = ^l_WT * ^WP
lP=WlT∗WP
W l T = l b T − 1 ∗ b W T − 1 ^l_WT = ^b_lT^{-1} * ^W_bT^{-1} WlT=lbT−1∗bWT−1
W l R = R − 1 = [ cos ( θ t + l b θ t ) sin ( θ t + l b θ t ) − sin ( θ t + l b θ t ) cos ( θ t + l b θ t ) ] ^l_WR = R^{-1} = \begin{bmatrix}\cos(\theta_t + ^b_l\theta_t)&\sin(\theta_t + ^b_l\theta_t)\\-\sin(\theta_t + ^b_l\theta_t) & \cos(\theta_t + ^b_l\theta_t)\end{bmatrix} WlR=R−1=[cos(θt+lbθt)−sin(θt+lbθt)sin(θt+lbθt)cos(θt+lbθt)]
W l t = − R − 1 t = − W l R ∗ ( R 1 ∗ t 2 + t 1 ) ^l_Wt = -R^{-1}t=-^l_WR*(R_1*t_2+t_1) Wlt=−R−1t=−WlR∗(R1∗t2+t1)
单个观测方程可以写为:
l P = W l T ∗ W P = W l R ∗ W P + W l t = W l R ∗ ( W P − R 1 ∗ t 2 − t 1 ) ^lP = ^l_WT * ^WP=^l_WR*^WP+^l_Wt=^l_WR*(^WP-R_1*t_2-t_1) lP=WlT∗WP=WlR∗WP+Wlt=WlR∗(WP−R1∗t2−t1)
[ r x i r y i ] = [ cos ( θ t + l b θ t ) sin ( θ t + l b θ t ) − sin ( θ t + l b θ t ) cos ( θ t + l b θ t ) ] ∗ ( [ M x i M y i ] − [ cos θ t − sin θ t sin θ t cos θ t ] ∗ [ l b x t l b y t ] − [ x t y t ] ) \begin{bmatrix}r_x^i \\ r_y^i\end{bmatrix} = \begin{bmatrix}\cos(\theta_t + ^b_l\theta_t) & \sin(\theta_t + ^b_l\theta_t)\\-\sin(\theta_t + ^b_l\theta_t) & \cos(\theta_t + ^b_l\theta_t)\end{bmatrix} * (\begin{bmatrix}M_x^i \\ M_y^i\end{bmatrix} - \begin{bmatrix}\cos\theta_t&-\sin\theta_t\\\sin\theta_t & \cos\theta_t\end{bmatrix} * \begin{bmatrix}^b_lx_t\\^b_ly_t\end{bmatrix} - \begin{bmatrix}x_t\\y_t\end{bmatrix}) [rxiryi]=[cos(θt+lbθt)−sin(θt+lbθt)sin(θt+lbθt)cos(θt+lbθt)]∗([MxiMyi]−[cosθtsinθt−sinθtcosθt]∗[lbxtlbyt]−[xtyt])
T 1 = [ cos ( θ t + l b θ t ) sin ( θ t + l b θ t ) − sin ( θ t + l b θ t ) cos ( θ t + l b θ t ) ] T_1 = \begin{bmatrix}\cos(\theta_t + ^b_l\theta_t) & \sin(\theta_t + ^b_l\theta_t)\\-\sin(\theta_t + ^b_l\theta_t) & \cos(\theta_t + ^b_l\theta_t)\end{bmatrix} T1=[cos(θt+lbθt)−sin(θt+lbθt)sin(θt+lbθt)cos(θt+lbθt)]
T 2 = [ M x i M y i ] − [ cos θ t − sin θ t sin θ t cos θ t ] ∗ [ l b x t l b y t ] − [ x t y t ] T_2 = \begin{bmatrix}M_x^i \\ M_y^i\end{bmatrix} - \begin{bmatrix}\cos\theta_t&-\sin\theta_t\\\sin\theta_t & \cos\theta_t\end{bmatrix} * \begin{bmatrix}^b_lx_t\\^b_ly_t\end{bmatrix}- \begin{bmatrix}x_t\\y_t\end{bmatrix} T2=[MxiMyi]−[cosθtsinθt−sinθtcosθt]∗[lbxtlbyt]−[xtyt]
关联上老地图的反光板:
单个观测对状态空间 X t X_t Xt的雅克比为:
H t i = [ A i B i C i ] 2 ∗ ( 2 ∗ N + 6 ) H_t^i = \begin{bmatrix}A_i & B_i & C_i\end{bmatrix}_{2*(2*N+6)} Hti=[AiBiCi]2∗(2∗N+6)
其中, A i A_i Ai是反光板观测对当前时刻位姿的偏导:
A i = [ ∂ r x i ∂ x t ∂ r x i ∂ y t ∂ r x i ∂ θ t ∂ r y i ∂ x t ∂ r y i ∂ y t ∂ r y i ∂ θ t ] 2 ∗ 3 A_i = \begin{bmatrix}\frac{\partial r_x^i }{ \partial x_t} & \frac{\partial r_x^i }{ \partial y_t} & \frac{\partial r_x^i }{ \partial \theta_t} \\ \frac{\partial r_y^i }{ \partial x_t} & \frac{\partial r_y^i }{ \partial y_t} & \frac{\partial r_y^i }{ \partial \theta_t}\end{bmatrix}_{2*3} Ai=⎣⎡∂xt∂rxi∂xt∂ryi∂yt∂rxi∂yt∂ryi∂θt∂rxi∂θt∂ryi⎦⎤2∗3
前两行两列为: A i 1 = − W l R A_i^{1} = -^l_WR Ai1=−WlR
后两行一列为: A i 2 = [ ∂ r x i ∂ θ t ∂ r y i ∂ θ t ] = ∂ T 1 ∂ θ t ∗ T 2 + T 1 ∗ ∂ T 2 ∂ θ t A_i^{2} = \begin{bmatrix}\frac{\partial r_x^i }{ \partial \theta_t} \\ \frac{\partial r_y^i }{ \partial \theta_t}\end{bmatrix}= \frac{\partial T_1 }{ \partial \theta_t} * T_2 + T_1 * \frac{\partial T_2 }{ \partial \theta_t} Ai2=[∂θt∂rxi∂θt∂ryi]=∂θt∂T1∗T2+T1∗∂θt∂T2
∂ T 1 ∂ θ t = [ − sin ( θ t + l b θ t ) cos ( θ t + l b θ t ) − cos ( θ t + l b θ t ) − sin ( θ t + l b θ t ) ] \frac{\partial T_1 }{ \partial \theta_t} =\begin{bmatrix}-\sin(\theta_t + ^b_l\theta_t) & \cos(\theta_t + ^b_l\theta_t)\\-\cos(\theta_t + ^b_l\theta_t) & -\sin(\theta_t + ^b_l\theta_t)\end{bmatrix} ∂θt∂T1=[−sin(θt+lbθt)−cos(θt+lbθt)cos(θt+lbθt)−sin(θt+lbθt)]
∂ T 2 ∂ θ t = − [ − sin θ t − cos θ t cos θ t − sin θ t ] ∗ [ l b x t l b y t ] \frac{\partial T_2 }{ \partial \theta_t} = - \begin{bmatrix}-\sin\theta_t&-\cos\theta_t\\\cos\theta_t & -\sin\theta_t\end{bmatrix} * \begin{bmatrix}^b_lx_t\\^b_ly_t\end{bmatrix} ∂θt∂T2=−[−sinθtcosθt−cosθt−sinθt]∗[lbxtlbyt]
B i B_i Bi是反光板观测对外参的偏导:
B i = [ ∂ r x i ∂ l b x t ∂ r x i ∂ l b y t ∂ r x i ∂ l b θ t ∂ r y i ∂ l b x t ∂ r y i ∂ l b y t ∂ r y i ∂ l b θ t ] 2 ∗ 3 B_i = \begin{bmatrix}\frac{\partial r_x^i }{ \partial ^b_lx_t} & \frac{\partial r_x^i }{ \partial ^b_ly_t} & \frac{\partial r_x^i }{ \partial ^b_l\theta_t} \\ \frac{\partial r_y^i }{ \partial ^b_lx_t} & \frac{\partial r_y^i }{ \partial ^b_ly_t} & \frac{\partial r_y^i }{ \partial ^b_l\theta_t}\end{bmatrix}_{2*3} Bi=⎣⎡∂lbxt∂rxi∂lbxt∂ryi∂lbyt∂rxi∂lbyt∂ryi∂lbθt∂rxi∂lbθt∂ryi⎦⎤2∗3
前两行两列为: B i 1 = − W l R ∗ R 1 B_i^1 = -^l_WR*R_1 Bi1=−WlR∗R1
后两行一列为: B i 2 = [ ∂ r x i ∂ l b θ t ∂ r y i ∂ l b θ t ] = ∂ T 1 ∂ l b θ t ∗ T 2 B_i^2 = \begin{bmatrix}\frac{\partial r_x^i }{ \partial ^b_l\theta_t} \\ \frac{\partial r_y^i }{ \partial ^b_l\theta_t}\end{bmatrix}= \frac{\partial T_1 }{ \partial ^b_l\theta_t} * T_2 Bi2=⎣⎡∂lbθt∂rxi∂lbθt∂ryi⎦⎤=∂lbθt∂T1∗T2
∂ T 1 ∂ l b θ t = [ − sin ( θ t + l b θ t ) cos ( θ t + l b θ t ) − cos ( θ t + l b θ t ) − sin ( θ t + l b θ t ) ] \frac{\partial T_1 }{ \partial ^b_l\theta_t} = \begin{bmatrix}-\sin(\theta_t + ^b_l\theta_t) & \cos(\theta_t + ^b_l\theta_t)\\-\cos(\theta_t + ^b_l\theta_t) & -\sin(\theta_t + ^b_l\theta_t)\end{bmatrix} ∂lbθt∂T1=[−sin(θt+lbθt)−cos(θt+lbθt)cos(θt+lbθt)−sin(θt+lbθt)]
C i C_i Ci是反光板观测对状态空间内的反光板的偏导:
C i = [ 0 ] 2 ∗ 2 N C_i = \left[0\right]_{2*2N} Ci=[0]2∗2N
关联上状态空间上的反光板
和上一个公式基本一样,需要注意的是,由于匹配的是状态空间上的反光板, C i C_i Ci是不一样的。
假如匹配的是状态空间内第 j j j个反光板,则有:
C i = [ 0 2 ∗ ( j − 1 ) W l R 0 2 ∗ ( N − j ) ] 2 ∗ 2 N C_i = \begin{bmatrix} 0_{2*{(j-1)}}& ^l_WR & 0_{2 * (N-j)}\end{bmatrix}_{2*2N} Ci=[02∗(j−1)WlR02∗(N−j)]2∗2N
关于更新等和定位EKF公式推导一样,后面不再详细说明。
这样就通过EKF完成了在线的轮速计外参的标定,在系统收敛的情况下(通过协方差阈值等手段),即可得到外参和反光板地图。