由雷达帧到相机
第
i
i
i个雷达帧的位姿
(
R
L
i
,
t
L
i
)
(R_{Li},t_{Li})
(RLi,tLi),这是在全局坐标系下对雷达坐标系的观测结果。因此对于第
i
i
i个雷达坐标系中的一点
P
L
P_{L}
PL,其在全局坐标系中的坐标
P
W
P_{W}
PW为:
P
W
=
R
L
i
P
L
+
t
L
i
P_{W}=R_{Li} P_{L}+t_{Li}
PW=RLiPL+tLi
雷达在相机位置处的标准ROS坐标系下的观测位姿为
(
R
R
,
t
R
)
(R_R,t_R)
(RR,tR),点
P
L
P_{L}
PL在位于第
i
i
i个相机位置处的标准ROS坐标系下的坐标
P
R
P_{R}
PR为:
P
R
=
R
R
P
L
+
t
R
P_{R}=R_R P_{L}+t_R
PR=RRPL+tR
相机在相机位置处的标准ROS坐标系下的观测位姿为
(
R
C
,
0
)
(R_C,0)
(RC,0),点
P
L
P_{L}
PL在位于第
i
i
i个相机坐标系下的坐标
P
C
P_{C}
PC为:
P
C
=
R
C
−
1
(
P
R
−
0
)
P_C=R_C^{-1}( P_R-0)
PC=RC−1(PR−0)
由相机坐标返回
P
R
=
R
C
P
C
=
R
R
P
L
+
t
R
=
R
R
R
L
i
−
1
(
P
W
−
t
L
i
)
+
t
R
P_R=R_C P_C=R_R P_{L}+t_R=R_R R_{Li}^{-1}(P_{W} - t_{Li})+t_R
PR=RCPC=RRPL+tR=RRRLi−1(PW−tLi)+tR
P
W
=
R
L
i
R
R
−
1
(
R
C
P
C
−
t
R
)
+
t
L
i
=
R
L
i
R
R
−
1
R
C
P
C
+
t
L
i
−
R
L
i
R
R
−
1
t
R
P_W = R_{Li} R_R^{-1} (R_C P_C-t_R) + t_{Li}= R_{Li} R_R^{-1} R_C P_C + t_{Li} - R_{Li} R_R^{-1} t_R
PW=RLiRR−1(RCPC−tR)+tLi=RLiRR−1RCPC+tLi−RLiRR−1tR
相机外参为
(
R
L
i
R
R
−
1
R
C
,
t
L
i
−
R
L
i
R
R
−
1
t
R
)
(R_{Li} R_R^{-1} R_C, t_{Li} - R_{Li} R_R^{-1} t_R)
(RLiRR−1RC,tLi−RLiRR−1tR)