激光雷达外参数自标定
激光雷达标定
下图为车载6个摄像头、1个360度激光雷达、5个毫米雷达、1个GPS及1个惯性测量传感器以及车体base坐标系的坐标系框图。
| 传感器类型 | X轴方向 | Y轴方向 | Z轴方向 | 原点 |
|---|---|---|---|---|
| Camera | 右 | 地 | 前 | |
| Radar | 前 | 左 | 天 | |
| Lidar | 右 | 前 | 天 | |
| GPS | 东 | 北 | 天 | |
| IMU | 前 | 左 | 天 | |
| 车体base坐标系 | 右 | 前 | 天 | 车辆后轴中心 |
左手坐标系：伸开我们的左手, 掌心向外, 大拇指与食指成90度, 中指、无名指和小指弯曲, 大拇指指向的方向就是X轴正方向, 食指指向的方向就是Y轴正方向, 中指、无名指和小指指向的方向就是Z轴正方向。用左手的大拇指指向轴的正方向，弯曲手指。那么手指所指示的方向即是轴的正旋转方向。
右手坐标系:伸开我们的右手, 掌心向内, 大拇指与食指成90度, 中指、无名指和小指弯曲, 大拇指指向的方向就是X轴正方向, 食指指向的方向就是Y轴正方向, 中指、无名指和小指指向的方向就是Z轴正方向。用右手的大拇指指向轴的正方向，弯曲手指。那么手指所指示的方向即是轴的正旋转方向。
左手坐标系和右手坐标系中, X轴和Y轴的方向是相同的, Z轴的方向相反。在计算机中通常使用的是左手坐标系，而数学中则通常使用右手坐标系。
| 传感器类型 | X轴方向 | Y轴方向 | Z轴方向 | 坐标系 |
|---|---|---|---|---|
| Camera | 右 | 地 | 前 | O X c Y c Z c OX_cY_cZ_c OXcYcZc |
| Radar | 前 | 左 | 天 | O X r Y r Z r OX_rY_rZ_r OXrYrZr |
假设毫米传感器在摄像机坐标系下的坐标为
X
o
f
f
s
e
t
Y
o
f
f
s
e
t
Z
o
f
f
s
e
t
X_{offset}Y_{offset}Z_{offset}
XoffsetYoffsetZoffset；
O
X
r
Y
r
Z
r
OX_rY_rZ_r
OXrYrZr坐标系在
O
X
c
Y
c
Z
c
OX_cY_cZ_c
OXcYcZc坐标系下的姿态为
(
α
,
β
,
γ
)
(\alpha,\beta,\gamma)
(α,β,γ)(俯仰角pitch、航向角yaw、翻滚角roll)；在
X
c
X_c
Xc轴的旋转矩阵为
R
x
R_x
Rx, 在
Y
c
Y_c
Yc轴的旋转矩阵为
R
y
R_y
Ry, 在
Z
c
Z_c
Zc轴的旋转矩阵为
R
z
R_z
Rz；则由前左天
O
X
r
Y
r
Z
r
OX_rY_rZ_r
OXrYrZr坐标系转右地前
O
X
c
Y
c
Z
c
OX_cY_cZ_c
OXcYcZc坐标系：
{
X
c
=
R
x
(
−
Y
r
)
+
X
o
f
f
s
e
t
Y
c
=
R
y
(
−
Z
r
)
+
Y
o
f
f
s
e
t
Z
c
=
R
z
(
X
r
)
+
Z
o
f
f
s
e
t
\begin{cases} X_c = R_x (- Y_r) + X_{offset} \\ Y_c = R_y (- Z_r) + Y_{offset} \\ Z_c = R_z ( X_r) + Z_{offset} \end{cases}
⎩⎪⎨⎪⎧Xc=Rx(−Yr)+XoffsetYc=Ry(−Zr)+YoffsetZc=Rz(Xr)+Zoffset
写成矩阵的形式：
[
X
c
Y
c
Z
c
]
=
{
[
1
0
0
0
c
o
s
(
α
)
s
i
n
(
α
)
0
−
s
i
n
(
α
)
c
o
s
(
α
)
]
[
c
o
s
(
β
)
0
−
s
i
n
(
β
)
0
1
0
s
i
n
(
β
)
0
c
o
s
(
β
)
]
[
c
o
s
(
γ
)
s
i
n
(
γ
)
0
−
s
i
n
(
γ
)
c
o
s
(
γ
)
0
0
0
1
]
}
{
[
0
−
1
0
0
0
−
1
1
0
0
]
∗
[
X
r
Y
r
Z
r
]
}
+
{
[
X
o
f
f
s
e
t
Y
o
f
f
s
e
t
Z
o
f
f
s
e
t
]
}
\left[ \begin{matrix} X_c \\ Y_c \\ Z_c \end{matrix} \right] = \left\{ \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos(\alpha) & sin(\alpha) \\ 0 & -sin(\alpha) & cos(\alpha) \end{matrix} \right] \left[ \begin{matrix} cos(\beta) & 0 & -sin(\beta) \\ 0 & 1 & 0 \\ sin(\beta) & 0 & cos(\beta) \end{matrix} \right] \left[ \begin{matrix} cos(\gamma) & sin(\gamma) & 0 \\ -sin(\gamma) & cos(\gamma) & 0 \\ 0 & 0 & 1 \end{matrix} \right] \right\} \left\{ \left[ \begin{matrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \\ \end{matrix} \right] * \left[ \begin{matrix} X_r \\ Y_r \\ Z_r \end{matrix} \right] \right\} + \left\{ \left[ \begin{matrix} X_{offset} \\ Y_{offset} \\ Z_{offset} \end{matrix} \right] \right\}
⎣⎡XcYcZc⎦⎤=⎩⎨⎧⎣⎡1000cos(α)−sin(α)0sin(α)cos(α)⎦⎤⎣⎡cos(β)0sin(β)010−sin(β)0cos(β)⎦⎤⎣⎡cos(γ)−sin(γ)0sin(γ)cos(γ)0001⎦⎤⎭⎬⎫⎩⎨⎧⎣⎡001−1000−10⎦⎤∗⎣⎡XrYrZr⎦⎤⎭⎬⎫+⎩⎨⎧⎣⎡XoffsetYoffsetZoffset⎦⎤⎭⎬⎫
接下来就是相机的标定原理了。
雷达转相机坐标系:
[
X
c
Y
c
Z
c
]
=
{
R
∣
T
}
{
[
0
−
1
0
0
0
−
1
1
0
0
]
∗
[
X
r
Y
r
Z
r
]
}
{
[
0
−
1
0
0
0
−
1
1
0
0
]
∗
[
X
r
Y
r
Z
r
]
}
+
{
[
X
o
f
f
s
e
t
Y
o
f
f
s
e
t
Z
o
f
f
s
e
t
]
}
\left[ \begin{matrix} X_c \\ Y_c \\ Z_c \end{matrix} \right] = \left\{ R|T \right\} \left\{ \left[ \begin{matrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \\ \end{matrix} \right] * \left[ \begin{matrix} X_r \\ Y_r \\ Z_r \end{matrix} \right] \right\} \left\{ \left[ \begin{matrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \\ \end{matrix} \right] * \left[ \begin{matrix} X_r \\ Y_r \\ Z_r \end{matrix} \right] \right\} + \left\{ \left[ \begin{matrix} X_{offset} \\ Y_{offset} \\ Z_{offset} \end{matrix} \right] \right\}
⎣⎡XcYcZc⎦⎤={R∣T}⎩⎨⎧⎣⎡001−1000−10⎦⎤∗⎣⎡XrYrZr⎦⎤⎭⎬⎫⎩⎨⎧⎣⎡001−1000−10⎦⎤∗⎣⎡XrYrZr⎦⎤⎭⎬⎫+⎩⎨⎧⎣⎡XoffsetYoffsetZoffset⎦⎤⎭⎬⎫
因为毫米波雷达的
Z
r
Z_r
Zr为0，也就时相机坐标系的
Y
c
Y_c
Yc为0，没有
Y
c
Y_c
Yc坐标轴。对上式进行化简可得:
[
X
c
Y
c
Z
c
]
=
{
[
c
o
s
(
β
)
0
−
s
i
n
(
β
)
0
1
0
s
i
n
(
β
)
0
c
o
s
(
β
)
]
}
{
[
0
−
1
0
0
0
−
1
1
0
0
]
∗
[
X
r
Y
r
Z
r
]
}
+
{
[
X
o
f
f
s
e
t
Y
o
f
f
s
e
t
Z
o
f
f
s
e
t
]
}
\left[ \begin{matrix} X_c \\ Y_c \\ Z_c \end{matrix} \right] =\left\{ \left[ \begin{matrix} cos(\beta) & 0 & -sin(\beta) \\ 0 & 1 & 0 \\ sin(\beta) & 0 & cos(\beta) \end{matrix} \right] \right\} \left\{ \left[ \begin{matrix} 0 & -1 & 0 \\ 0 & 0 & -1 \\ 1 & 0 & 0 \\ \end{matrix} \right] * \left[ \begin{matrix} X_r \\ Y_r \\ Z_r \end{matrix} \right] \right\} + \left\{ \left[ \begin{matrix} X_{offset} \\ Y_{offset} \\ Z_{offset} \end{matrix} \right] \right\}
⎣⎡XcYcZc⎦⎤=⎩⎨⎧⎣⎡cos(β)0sin(β)010−sin(β)0cos(β)⎦⎤⎭⎬⎫⎩⎨⎧⎣⎡001−1000−10⎦⎤∗⎣⎡XrYrZr⎦⎤⎭⎬⎫+⎩⎨⎧⎣⎡XoffsetYoffsetZoffset⎦⎤⎭⎬⎫
相机坐标系转像素坐标系:
z
c
1
(
u
v
1
)
=
{
[
1
d
x
0
u
0
0
1
d
y
v
0
0
0
1
]
[
f
0
0
0
f
0
0
0
1
]
}
{
[
X
c
Y
c
Z
c
]
}
z_{c1} \left( \begin{matrix} u \\ v \\ 1 \end{matrix} \right) = \left\{ \left[ \begin{matrix} \frac{1}{d_x} & 0 & u_0 \\ 0 & \frac{1}{d_y} & v_0 \\ 0 & 0 & 1 \end{matrix} \right] \left[ \begin{matrix} f & 0 & 0 \\ 0 & f & 0 \\ 0 & 0 & 1 \end{matrix} \right] \right\} \left\{ \left[ \begin{matrix} X_c \\ Y_c \\ Z_c \end{matrix} \right] \right\}
zc1⎝⎛uv1⎠⎞=⎩⎨⎧⎣⎡dx1000dy10u0v01⎦⎤⎣⎡f000f0001⎦⎤⎭⎬⎫⎩⎨⎧⎣⎡XcYcZc⎦⎤⎭⎬⎫
一般两坐标进行旋转变换时沿逆时针旋转。
有点P(Pa,PYa)[(Xa，Ya)]，当坐标由 x –> y 旋转 b(θ） 度后，求该点在新坐标轴的坐标是多少。
有点P（Xa，Ya），当坐标由 x –> y 旋转 θ 度后，求该点在新坐标轴的坐标是多少。
三维旋转，需要先搞清楚正、负方向（使用的是右手法则，在二维平面增加一维z，它的正方向朝向屏幕外）
绕x轴进行旋转（在yz平面逆时针旋转）:
绕y轴进行旋转（在zx平面逆时针旋转）:
绕z轴进行旋转（在xy平面逆时针旋转）:
参考:https://mathworld.wolfram.com/RotationMatrix.html
