- 相机的
平移向量
一般标定到imu坐标系或者车身坐标系,欧拉角 y a w 、 p i t c h 、 r o l l \color{red}yaw、pitch、roll yaw、pitch、roll是相对于前向相机坐标系的位姿; 前向相机坐标系
:不特指某个具体相机的坐标系,而是指面向车辆前方并且欧拉角 y a w 、 p i t c h 、 r o l l \color{red}yaw、pitch、roll yaw、pitch、roll都为0的标准相机坐标系。方向是: X X X右正, Y Y Y下正, Z Z Z前正;对于每个相机,都有一个对应的前向相机坐标系,该前向相机坐标系和这个歪相机坐标系的原点是重合的。歪相机坐标系
:指代某个相机的坐标系,实际项目中,该相机相对于前向相机坐标系
绝对是有姿态角(欧拉角)的,本文都叫它歪相机坐标系,也是为了描述更简洁。
这里说一下,imu坐标系
和车身坐标系
的原点可能不重合,但是我们项目的相机外参标定模块为了不跟车型关联,就把imu坐标系
和车身坐标系
的原点设为重合,后面要使用到相机外参的节点再通过imu2car
的参数变换。
我项目中标定的平移向量的
x
,
y
,
z
\color{red}x,y,z
x,y,z 是标定到imu坐标系
。
以下是相机外参配置文件
中的平移向量
的值:
translation:
# x是该相机在imu坐标系下,距离imu中心的横向距离,由于该相机在imu右边,所以是正数,1.08m
x: 1.0800000429153442e+00
# y是该相机在imu坐标系下,距离imu中心的纵向距离,由于该相机在imu后面,所以是负数,-1m
y: -1.0290000438690186e+00
# z是该相机在imu坐标系下,距离imu中心的高度距离,由于该相机在imu下面,所以是负数,-0.070m即7cm
z: -7.0000000298023224e-02
(1) 标定时:
补充:
由于版本更新问题,之前版本相机是标定在车身坐标系。目前版本是更新在imu坐标系,确切的说,是标定在INS坐标系。INS坐标系与imu坐标系有一点偏差的角度。车身坐标系与INS坐标系是完完全全的90°旋转的关系。
由于之前版本更新,代码改动为了尽量少该代码,就还是使用了imu2car这个中间参数,这就引入了误差。
下面介绍的imu坐标系其实应该是INS坐标系。并且下面的转换通过imu2car这个变换矩阵进行中转,由于imu2car的旋转角度有轻微误差,不是准确的90°旋转关系,因此会引进误差。所以,后续把工程代码中去掉了imu2car,直接使用camera2imu,再求逆,得到了imu2camera。那么imu坐标系的激光点云就通过这个imu2camera变换矩阵,得到了激光点云在camera坐标系下的点,然后再通过相机内参矩阵变换到像素坐标系。
标定时,是3D激光点云投影到2D像素平面,我们通过读取相机外参配置文件中的欧拉角值和平移向量,得到外参初值,然后手动调节这六个外参,使得激光点云经过imu2car
的外参以及相机内外参
,投影到像素平面,调参直到障碍物的点云和像素平面的对应障碍物完美重合,就得到了标定后的相机外参。
具体实现细节是:
- 1.由于相机位姿
欧拉角
是相机相对于前向相机坐标系
的姿态角,平移向量
是相机在imu坐标系
下的坐标,所以能很容易地根据配置文件或手动调节后外参欧拉角
,得到歪相机的某点投影到前向相机的旋转矩阵camera2front
; - 2.再根据
前向相机坐标系
到car坐标系
的坐标系变换
得到front2car
。这里得到的车身坐标系并非真正的车身坐标系,而是原点和前向相机坐标系重合的车身坐标系。 - 3.再利用相机外参的
平移向量
对上一步得到的点进行平移,就得到了车身坐标系
的变换矩阵
。在代码中,是在R=front2car * camera2front
后,然后把旋转矩阵R
与平移向量整合成一个4x4
齐次变换矩阵camera_to_car_augm
,然后对这个齐次矩阵求逆
,就得到了车身坐标系
某点投影到歪相机坐标系的变换矩阵car2camera_ = camera_to_car_augm.inverse();
, - 4.由于车身坐标系的激光点云是通过imu坐标系的激光点云通过
imu2car
的变换矩阵变换得到,所以先通过pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_car_ptr_, imu_to_car_);
得到车身坐标系的激光点云,那么,我们就可以联合上一步的car2camera_
,得到imu2camera
。具体推导见下面的公式推导。 - 5.再将上一步得到的
歪相机坐标系
下的点通过相机内参矩阵
进行变换,(以及畸变处理),就可以把该歪相机坐标系
的点投影到像素坐标系中.
综上,就是利用这6个相机外参先得到歪相机坐标系的点 投影变换到car坐标系
的变换矩阵,然后再对这个变换矩阵求逆,就得到了car2camera
,得到激光点云
投影到歪相机坐标系
的点之后,再通过相机内参矩阵
,就可以把该点投影到像素坐标系中了。
标定时,把车身坐标系的激光点云变换到歪相机坐标系:
[
P
c
a
r
1
]
=
T
f
r
o
n
t
c
a
r
∗
T
c
a
m
e
r
a
f
r
o
n
t
∗
[
P
c
a
m
e
r
a
1
]
=
[
R
f
r
o
n
t
c
a
r
t
f
r
o
n
t
c
a
r
0
T
1
]
∗
[
R
c
a
m
e
r
a
f
r
o
n
t
t
c
a
m
e
r
a
f
r
o
n
t
0
T
1
]
∗
[
P
c
a
m
e
r
a
1
]
\color{red} \begin{aligned} \begin{bmatrix} P_{car} \\\\ 1\end{bmatrix} &=T^{car}_{front}*T^{front}_{camera}*\begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} \\\\ &=\begin{bmatrix} R^{car}_{front}&t^{car}_{front}\\\\ 0^T&1\end{bmatrix}*\begin{bmatrix} R^{front}_{camera}&t^{front}_{camera}\\\\ 0^T&1\end{bmatrix}*\begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} \end{aligned}
⎣⎡Pcar1⎦⎤=Tfrontcar∗Tcamerafront∗⎣⎡Pcamera1⎦⎤=⎣⎡Rfrontcar0Ttfrontcar1⎦⎤∗⎣⎡Rcamerafront0Ttcamerafront1⎦⎤∗⎣⎡Pcamera1⎦⎤
由于
t
c
a
m
e
r
a
f
r
o
n
t
=
0
⃗
3
∗
1
\color{red}t^{front}_{camera}=\vec{0}_{3*1}
tcamerafront=03∗1,所以从歪camera坐标系点
到车辆坐标系点
的变换矩阵就是:
T
c
a
m
e
r
a
c
a
r
=
[
R
f
r
o
n
t
c
a
r
t
f
r
o
n
t
c
a
r
0
T
1
]
∗
[
R
c
a
m
e
r
a
f
r
o
n
t
0
⃗
0
T
1
]
=
[
R
f
r
o
n
t
c
a
r
∗
R
c
a
m
e
r
a
f
r
o
n
t
t
f
r
o
n
t
c
a
r
0
T
1
]
(1)
\color{red} \begin{aligned} T_{camera}^{car}&=\begin{bmatrix} R^{car}_{front}&t^{car}_{front}\\\\ 0^T&1\end{bmatrix}*\begin{bmatrix} R^{front}_{camera}&\vec{0}\\\\ 0^T&1\end{bmatrix}\\ \\ &=\begin{bmatrix} R^{car}_{front}*R^{front}_{camera}&t^{car}_{front}\\ \\0^T&1\end{bmatrix} \end{aligned} \tag{1}
Tcameracar=⎣⎡Rfrontcar0Ttfrontcar1⎦⎤∗⎣⎡Rcamerafront0T01⎦⎤=⎣⎡Rfrontcar∗Rcamerafront0Ttfrontcar1⎦⎤(1)
然后再对
T
c
a
m
e
r
a
c
a
r
\color{red}T_{camera}^{car}
Tcameracar求逆,即得到把车身坐标系的激光点云
变换到歪相机坐标系
的变换矩阵
T
c
a
r
c
a
m
e
r
a
\color{red}T_{car}^{camera}
Tcarcamera。
相机标定模块输入的激光点云是imu坐标系
的激光点云。
t
f
r
o
n
t
i
m
u
\color{red}t^{imu}_{front}
tfrontimu是front相机坐标系原点
相对imu坐标系原点
的偏移向量,而
t
f
r
o
n
t
c
a
r
\color{red}t^{car}_{front}
tfrontcar是front相机坐标系原点
相对car坐标系原点
的偏移向量。
本模块中直接把
t
f
r
o
n
t
i
m
u
\color{red}t^{imu}_{front}
tfrontimu调换x、y方向和正负
得到
t
f
r
o
n
t
c
a
r
\color{red}t^{car}_{front}
tfrontcar,这样做就是设定了imu坐标系原点
和car坐标系原点
重合,即
t
i
m
u
c
a
r
=
0
⃗
\color{red}t^{car}_{imu}=\vec{0}
timucar=0。
下面是imu坐标系下
的激光点云的某点,转换到车辆坐标系下的公式:
[
P
c
a
r
1
]
=
[
R
i
m
u
c
a
r
t
i
m
u
c
a
r
0
T
1
]
∗
[
P
i
m
u
1
]
\color{red} \begin{bmatrix} P_{car}\\\\1\end{bmatrix}=\begin{bmatrix} R^{car}_{imu}&t^{car}_{imu}\\\\ 0^T&1\end{bmatrix}*\begin{bmatrix} P_{imu}\\\\1\end{bmatrix}
⎣⎡Pcar1⎦⎤=⎣⎡Rimucar0Ttimucar1⎦⎤∗⎣⎡Pimu1⎦⎤
又因为前面推导的歪camera坐标系点
到车辆坐标系点
下的公式为:
[
P
c
a
r
1
]
=
[
R
f
r
o
n
t
c
a
r
∗
R
c
a
m
e
r
a
f
r
o
n
t
t
f
r
o
n
t
c
a
r
0
T
1
]
∗
[
P
c
a
m
e
r
a
1
]
\color{red} \begin{aligned} \begin{bmatrix} P_{car}\\\\1\end{bmatrix}&=\begin{bmatrix} R^{car}_{front}*R^{front}_{camera}&t^{car}_{front}\\ \\0^T&1\end{bmatrix}*\begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} \end{aligned}
⎣⎡Pcar1⎦⎤=⎣⎡Rfrontcar∗Rcamerafront0Ttfrontcar1⎦⎤∗⎣⎡Pcamera1⎦⎤
联立上面两个等式,并移项,可以得到下式,下式是imu坐标系下
的激光点云的某点,转换到歪camera坐标系
下的公式:
[
P
c
a
m
e
r
a
1
]
=
[
R
f
r
o
n
t
c
a
m
e
r
a
∗
R
c
a
r
f
r
o
n
t
∗
R
i
m
u
c
a
r
R
f
r
o
n
t
c
a
m
e
r
a
∗
R
c
a
r
f
r
o
n
t
∗
(
t
i
m
u
c
a
r
−
t
f
r
o
n
t
c
a
r
)
0
T
1
]
∗
[
P
i
m
u
1
]
\color{red} \begin{aligned} \begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} =\begin{bmatrix} R^{camera}_{front}*R^{front}_{car}*R^{car}_{imu}&R^{camera}_{front}*R^{front}_{car}*(t_{imu}^{car}-t^{car}_{front})\\ \\0^T&1\end{bmatrix} *\begin{bmatrix} P_{imu}\\\\1\end{bmatrix} \end{aligned}
⎣⎡Pcamera1⎦⎤=⎣⎡Rfrontcamera∗Rcarfront∗Rimucar0TRfrontcamera∗Rcarfront∗(timucar−tfrontcar)1⎦⎤∗⎣⎡Pimu1⎦⎤
可得到从imu坐标系
的激光点云的某点到歪camera坐标系点
的变换矩阵:
T
i
m
u
c
a
m
e
r
a
=
[
R
f
r
o
n
t
c
a
m
e
r
a
∗
R
c
a
r
f
r
o
n
t
∗
R
i
m
u
c
a
r
,
R
f
r
o
n
t
c
a
m
e
r
a
∗
R
c
a
r
f
r
o
n
t
∗
(
t
i
m
u
c
a
r
−
t
f
r
o
n
t
c
a
r
)
0
T
,
1
]
(2)
\color{red} \begin{aligned} T_{imu}^{camera}=\begin{bmatrix} R^{camera}_{front}*R^{front}_{car}*R^{car}_{imu} &,&R^{camera}_{front}*R^{front}_{car}*(t_{imu}^{car}-t^{car}_{front})\\ \\0^T&,&1\end{bmatrix} \tag{2} \end{aligned}
Timucamera=⎣⎡Rfrontcamera∗Rcarfront∗Rimucar0T,,Rfrontcamera∗Rcarfront∗(timucar−tfrontcar)1⎦⎤(2)
公式(2)的变换矩阵
T
i
m
u
c
a
m
e
r
a
\color{red}T_{imu}^{camera}
Timucamera,就是把imu坐标系
下的激光点云转化到歪相机坐标系
点的变换矩阵。
对于歪相机坐标系的点转化到像素坐标系点的变换矩阵,就是内参矩阵,这里不再介绍。
因为变换矩阵的标准格式为:
T
i
m
u
c
a
m
e
r
a
=
[
R
i
m
u
c
a
m
e
r
a
,
t
i
m
u
c
a
m
e
r
a
0
T
,
1
]
\color{red} T_{imu}^{camera} =\begin{bmatrix} R^{camera}_{imu} &,&t^{camera}_{imu}\\ \\0^T&,&1\end{bmatrix}
Timucamera=⎣⎡Rimucamera0T,,timucamera1⎦⎤
所以:
R i m u c a m e r a = R f r o n t c a m e r a ∗ R c a r f r o n t ∗ R i m u c a r (3) \color{red} R^{camera}_{imu} =R^{camera}_{front}*R^{front}_{car}*R^{car}_{imu} \tag{3} Rimucamera=Rfrontcamera∗Rcarfront∗Rimucar(3)
t
i
m
u
c
a
m
e
r
a
=
R
f
r
o
n
t
c
a
m
e
r
a
∗
R
c
a
r
f
r
o
n
t
∗
(
t
i
m
u
c
a
r
−
t
f
r
o
n
t
c
a
r
)
(4)
\color{red} t^{camera}_{imu} =R^{camera}_{front}*R^{front}_{car}*(t_{imu}^{car}-t^{car}_{front}) \tag{4}
timucamera=Rfrontcamera∗Rcarfront∗(timucar−tfrontcar)(4)
公式(4)并不直接是相机外参文件中的偏移向量,公式(4)表示imu相对于camera的偏移向量,所以需要把相机外参文件中camera相对于imu的偏移向量
(
x
外
参
,
y
外
参
,
z
外
参
)
(x_{外参},y_{外参},z_{外参})
(x外参,y外参,z外参)转化为公式(4),变为了
(
−
x
外
参
,
z
外
参
,
−
y
外
参
)
(-x_{外参},z_{外参},-y_{外参})
(−x外参,z外参,−y外参)。
到这里,我们就只需要考虑上面公式(4)的
t
i
m
u
c
a
r
−
t
f
r
o
n
t
c
a
r
\color{red}t_{imu}^{car}-t^{car}_{front}
timucar−tfrontcar,通过画图:
可知
t
i
m
u
c
a
r
−
t
f
r
o
n
t
c
a
r
\color{red}t_{imu}^{car}-t^{car}_{front}
timucar−tfrontcar是在car坐标系
下从front相机坐标系原点
指向imu坐标系原点
的向量,就是imu坐标系原点
相对于front相机坐标系原点
在car坐标系
下偏移,只不过这个偏移向量的数值不是在imu坐标系
下的向量数值,而是在car坐标系
下。
代码中是把相机外参文件
中
(
x
,
y
,
z
)
(x,y,z)
(x,y,z)表示的偏移向量
t
f
r
o
n
t
i
m
u
\color{red}t_{front}^{imu}
tfrontimu,在没有考虑imu2car偏移向量的情况下,直接转化为了公式(1)中歪camera2car
变换矩阵
T
歪
c
a
m
e
r
a
c
a
r
\color{red}T_{歪camera}^{car}
T歪cameracar中的平移向量
t
f
r
o
n
t
c
a
r
\color{red}t^{car}_{front}
tfrontcar, 也就是说,把imu坐标系
下的平移向量
t
f
r
o
n
t
i
m
u
=
[
x
0
y
0
z
0
]
\color{red}t_{front}^{imu}=\begin{bmatrix} x_0\\ y_0\\z_0\end{bmatrix}
tfrontimu=⎣⎡x0y0z0⎦⎤转化到车身坐标系
下,不考虑imu2car偏移,就变成了
t
f
r
o
n
t
c
a
r
=
[
y
0
−
x
0
z
0
]
\color{red}t^{car}_{front}=\begin{bmatrix} y_0\\ -x_0\\z_0\end{bmatrix}
tfrontcar=⎣⎡y0−x0z0⎦⎤。
这么做是默认imu和car坐标系原点重合,所以这么做是有问题的,当imu和car之间有偏移的时候,就会出错,只是目前所有车的imu和car原点都重合,所以不影响。以后有空需要把代码中这个bug补上。
下面这个图别看,是我以前的错误想法,留着让我自己更清晰。
(2)相机障碍物后处理时(camera_process),把车身坐标系的激光点云障碍物转化到歪相机坐标系:
相机后处理模块的输入是车身坐标系的激光点云障碍物。
[
P
c
a
r
1
]
=
T
i
m
u
c
a
r
∗
T
c
a
m
e
r
a
i
m
u
∗
[
P
c
a
m
e
r
a
1
]
=
[
R
i
m
u
c
a
r
t
i
m
u
c
a
r
0
T
1
]
∗
[
R
f
r
o
n
t
i
m
u
∗
R
c
a
m
e
r
a
f
r
o
n
t
t
f
r
o
n
t
i
m
u
0
T
1
]
∗
[
P
c
a
m
e
r
a
1
]
=
[
R
i
m
u
c
a
r
∗
R
f
r
o
n
t
i
m
u
∗
R
c
a
m
e
r
a
f
r
o
n
t
R
i
m
u
c
a
r
∗
t
f
r
o
n
t
i
m
u
+
t
i
m
u
c
a
r
0
T
1
]
∗
[
P
c
a
m
e
r
a
1
]
\color{red} \begin{aligned} \begin{bmatrix} P_{car} \\\\ 1\end{bmatrix} &=T^{car}_{imu}*T^{imu}_{camera}*\begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} \\\\ &=\begin{bmatrix} R^{car}_{imu}&t^{car}_{imu}\\\\ 0^T&1\end{bmatrix}* \begin{bmatrix} R^{imu}_{front}*R^{front}_{camera}&t^{imu}_{front}\\\\ 0^T&1\end{bmatrix}*\begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} \\\\ &=\begin{bmatrix} R^{car}_{imu}*R^{imu}_{front}*R^{front}_{camera}&R^{car}_{imu}*t^{imu}_{front}+t^{car}_{imu}\\\\ 0^T&1\end{bmatrix}*\begin{bmatrix} P_{camera}\\ \\1\end{bmatrix} \end{aligned}
⎣⎡Pcar1⎦⎤=Timucar∗Tcameraimu∗⎣⎡Pcamera1⎦⎤=⎣⎡Rimucar0Ttimucar1⎦⎤∗⎣⎡Rfrontimu∗Rcamerafront0Ttfrontimu1⎦⎤∗⎣⎡Pcamera1⎦⎤=⎣⎡Rimucar∗Rfrontimu∗Rcamerafront0TRimucar∗tfrontimu+timucar1⎦⎤∗⎣⎡Pcamera1⎦⎤
得到从歪camera坐标系点
到车辆坐标系点
的变换矩阵就是:
T
c
a
m
e
r
a
c
a
r
=
[
R
i
m
u
c
a
r
∗
R
f
r
o
n
t
i
m
u
∗
R
c
a
m
e
r
a
f
r
o
n
t
R
i
m
u
c
a
r
∗
t
f
r
o
n
t
i
m
u
+
t
i
m
u
c
a
r
0
T
1
]
(3)
\color{red}T_{camera}^{car}= \begin{bmatrix}R^{car}_{imu}*R^{imu}_{front}*R^{front}_{camera}&R^{car}_{imu}*t^{imu}_{front}+t^{car}_{imu}\\\\ 0^T&1\end{bmatrix} \tag{3}
Tcameracar=⎣⎡Rimucar∗Rfrontimu∗Rcamerafront0TRimucar∗tfrontimu+timucar1⎦⎤(3)
然后再对
T
c
a
m
e
r
a
c
a
r
\color{red}T_{camera}^{car}
Tcameracar求逆,即得到把车身坐标系的激光点云
变换到歪相机坐标系
的变换矩阵
T
c
a
r
c
a
m
e
r
a
\color{red}T_{car}^{camera}
Tcarcamera。
疑问: 我们的目的是求车身坐标系
某点投影到歪相机坐标系
的变换矩阵car2camera
,为什么要通过求camera2car
的逆来间接求变换矩阵car2camera
,而不直接求车身坐标系
某点投影到歪相机坐标系
的变换矩阵car2camera
呢?
答: 因为从pixel坐标系点
映射到车辆坐标系
的公式中会用到的平移向量,例如相机相对与imu坐标系的平移向量,imu2car
的偏移向量,都是直接从相机配置文件和imu2car.yaml
中读取的,而这些参数都是从歪camera坐标系点
到car坐标系点
直接使用的。你如果不直接使用他们,而是做相应的反变换(从车辆坐标系
映射到pixel坐标系点
)也可以,但是总得选择一个方向,而直接使用标定得到的参数是最方便的,旋转矩阵是可以直接反变换,但是平移向量就并非直接相加减这么简单就是反变换,平移向量会涉及到乘法(自己推导一下公式就明白了),所以通过先求camera2car
,再求它的逆来间接求变换矩阵car2camera
。
(3) 像素坐标系某2D点反投影到3D车身坐标系时:
这里是利用已经标定好的相机内外参。
下面的
z
c
\color{red}z_c
zc 是车身坐标某点
在歪相机坐标系
的
z
\color{red}z
z 坐标,方向向前(但不是前向相机坐标系的正前方,它是歪的)。
从像素坐标系的某点反投影到车身坐标系的地面某点的公式,可参考我另一篇博客【自动驾驶】像素坐标系2D -> 3D车身坐标系 (附代码),也可以自己推导,如下:
P
c
a
r
=
z
c
∗
R
f
r
o
n
t
c
a
r
∗
R
歪
c
a
m
e
r
a
f
r
o
n
t
∗
K
−
1
∗
P
u
,
v
+
R
i
m
u
c
a
r
∗
t
f
r
o
n
t
i
m
u
+
t
i
m
u
c
a
r
=
[
x
c
a
r
y
c
a
r
z
c
a
r
]
\color{red}P_{car}=z_c*R^{car}_{front}*R^{front}_{歪camera}*K^{-1}*P_{u,v}+R^{car}_{imu}*{t}^{imu}_{front}+t^{car}_{imu}=\begin{bmatrix} x_{car}\\ y_{car} \\ z_{car}\\\end{bmatrix}
Pcar=zc∗Rfrontcar∗R歪camerafront∗K−1∗Pu,v+Rimucar∗tfrontimu+timucar=⎣⎡xcarycarzcar⎦⎤
所以只有三个值是未知数:
z
c
、
x
c
a
r
、
y
c
a
r
\color{red}z_c、x_{car}、y_{car}
zc、xcar、ycar,可通过下面方法解出这三个值:
- 1.通过地面上的某点在
车身坐标系
的高度值 z c a r \color{red}z_{car} zcar已知,对公式(10)的第三维度
的等式求解,求出在地面上的某点P在相机坐标系
下的深度值 z c \color{red}z_c zc; - 2.然后就可以利用 z c \color{red}z_c zc,根据公式(10)求解出 ( x c a r , y c a r ) \color{red}(x_{car},y_{car}) (xcar,ycar),这样就得到了像素坐标系的某2D像素点 P ( u , v ) P(u,v) P(u,v)反投影到3D车身坐标系下的点 ( x c a r , y c a r , z c a r ) \color{red}(x_{car}, y_{car}, z_{car}) (xcar,ycar,zcar)。
标定过程
与像素坐标系某2D点反投影到3D车身坐标系
的异同点:
无论是标定
时,还是像素坐标系某2D点反投影到3D车身坐标系
时,都是先得到camera2car
的变换矩阵。不同的是, 像素坐标系某2D点反投影到3D车身坐标系
时是直接使用camera2car
,而标定时需要对camera2car
变换矩阵求逆得到car2camera
。
特别需要注意的是:
从车身坐标系
到相机坐标系
的平移向量
,与从相机坐标系
到车身坐标系
的平移向量
的数值并不是相反数。
因为涉及到旋转矩阵的逆运算,这里是重中之重。
由于camera是标定到imu坐标系(和车身坐标系原点重合)中,所以相机外参配置文件中的平移向量
是,这个camera
在imu坐标系
下的坐标, 并不是imu
在camera
坐标系的坐标。
如果是从车身坐标系
转化到camera坐标系
,就也直接使用配置文件的偏移向量,只不过后面需要对求逆。
带平移向量的变换矩阵求逆公式推导:
从点
P
P
P经过变换矩阵
T
T
T得到
P
′
P'
P′点:
P
′
=
T
∗
P
=
[
R
t
⃗
0
T
1
]
∗
P
P'=T*P=\begin{bmatrix} R&\vec{t} \ \\ 0^T&1\end{bmatrix}*P
P′=T∗P=[R0Tt 1]∗P
求逆变换:
P
=
T
−
1
∗
P
′
=
[
R
−
1
−
R
−
1
t
⃗
0
T
1
]
∗
P
′
P=T^{-1}*P'=\begin{bmatrix} R^{-1}&-R^{-1}\vec{t} \ \\ 0^T&1\end{bmatrix}*P'
P=T−1∗P′=[R−10T−R−1t 1]∗P′
由于旋转矩阵都是正交矩阵,所以
R
−
1
=
R
T
R^{-1}=R^T
R−1=RT,因此,上面逆变换可以写成这样:
P
=
T
−
1
∗
P
′
=
[
R
T
−
R
T
t
⃗
0
T
1
]
∗
P
′
P=T^{-1}*P'=\begin{bmatrix} R^T&-R^T\vec{t} \ \\ 0^T&1\end{bmatrix}*P'
P=T−1∗P′=[RT0T−RTt 1]∗P′
逆变换矩阵:
T
−
1
=
[
R
T
−
R
T
t
⃗
0
T
1
]
T^{-1}=\begin{bmatrix} R^T&-R^T\vec{t} \ \\ 0^T&1\end{bmatrix}
T−1=[RT0T−RTt 1]