【自动驾驶】31.【相机外参标定】、【相机障碍物后处理】【地面的2D点反投影到3D】的过程对比

• 相机的平移向量一般标定到imu坐标系或者车身坐标系，欧拉角 $yaw、pitch、roll$是相对于前向相机坐标系的位姿；
• 前向相机坐标系：不特指某个具体相机的坐标系，而是指面向车辆前方并且欧拉角$yaw、pitch、roll$都为0的标准相机坐标系。方向是：$X$右正，$Y$下正，$Z$前正；对于每个相机，都有一个对应的前向相机坐标系，该前向相机坐标系和这个歪相机坐标系的原点是重合的。
• 歪相机坐标系：指代某个相机的坐标系，实际项目中，该相机相对于前向相机坐标系绝对是有姿态角(欧拉角)的，本文都叫它歪相机坐标系，也是为了描述更简洁。

这里说一下，imu坐标系和车身坐标系的原点可能不重合，但是我们项目的相机外参标定模块为了不跟车型关联，就把imu坐标系和车身坐标系的原点设为重合，后面要使用到相机外参的节点再通过imu2car的参数变换。

我项目中标定的平移向量的 $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，得到激光点云投影到歪相机坐标系的点之后，再通过相机内参矩阵，就可以把该点投影到像素坐标系中了。

标定时，把车身坐标系的激光点云变换到歪相机坐标系：

$$\begin{array}{rl}\left[\begin{array}{c}{P}_{car}\\ \\ 1\end{array}\right]& =T_{front}^{car}\ast T_{camera}^{front}\ast \left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]\\ & \\ & =\left[\begin{array}{cc}{R}_{front}^{car}& t_{front}^{car}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{cc}{R}_{camera}^{front}& t_{camera}^{front}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]\end{array}$$
由于$t_{camera}^{front}={\vec{0}}_{3\ast 1}$,所以从歪camera坐标系点到车辆坐标系点的变换矩阵就是：
$$\begin{array}{rl}{T}_{camera}^{car}& =\left[\begin{array}{cc}{R}_{front}^{car}& t_{front}^{car}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{cc}{R}_{camera}^{front}& \vec{0}\\ & \\ 0^T& 1\end{array}\right]\\ & \\ & =\left[\begin{array}{cc}{R}_{front}^{car}\ast R_{camera}^{front}& t_{front}^{car}\\ & \\ 0^T& 1\end{array}\right]\end{array}\text{\hspace{1em}(1)}$$
然后再对$T_{camera}^{car}$求逆，即得到把车身坐标系的激光点云变换到歪相机坐标系的变换矩阵 $T_{car}^{camera}$。

相机标定模块输入的激光点云是imu坐标系的激光点云。 $t_{front}^{imu}$是front相机坐标系原点相对imu坐标系原点的偏移向量，而$t_{front}^{car}$是front相机坐标系原点相对car坐标系原点的偏移向量。
本模块中直接把 $t_{front}^{imu}$调换x、y方向和正负得到 $t_{front}^{car}$，这样做就是设定了imu坐标系原点和car坐标系原点重合，即 $t_{imu}^{car}=\vec{0}$。

下面是imu坐标系下的激光点云的某点，转换到车辆坐标系下的公式：
$$\left[\begin{array}{c}{P}_{car}\\ \\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{imu}^{car}& t_{imu}^{car}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{imu}\\ \\ 1\end{array}\right]$$
又因为前面推导的歪camera坐标系点到车辆坐标系点下的公式为：
$$\begin{array}{rl}\left[\begin{array}{c}{P}_{car}\\ \\ 1\end{array}\right]& =\left[\begin{array}{cc}{R}_{front}^{car}\ast R_{camera}^{front}& t_{front}^{car}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]\end{array}$$
联立上面两个等式，并移项，可以得到下式，下式是imu坐标系下的激光点云的某点，转换到歪camera坐标系下的公式：
$$\begin{array}{r}\left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_{front}^{camera}\ast R_{car}^{front}\ast R_{imu}^{car}& R_{front}^{camera}\ast R_{car}^{front}\ast (t_{imu}^{car}-t_{front}^{car})\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{imu}\\ \\ 1\end{array}\right]\end{array}$$
可得到从imu坐标系的激光点云的某点到歪camera坐标系点的变换矩阵：
$$\begin{array}{r}{T}_{imu}^{camera}=\left[\begin{array}{ccc}{R}_{front}^{camera}\ast R_{car}^{front}\ast R_{imu}^{car}& ,& R_{front}^{camera}\ast R_{car}^{front}\ast (t_{imu}^{car}-t_{front}^{car})\\ & & \\ 0^T& ,& 1\end{array}\right]\end{array}\text{\hspace{1em}(2)}$$
公式(2)的变换矩阵 $T_{imu}^{camera}$，就是把imu坐标系下的激光点云转化到歪相机坐标系点的变换矩阵。

对于歪相机坐标系的点转化到像素坐标系点的变换矩阵，就是内参矩阵，这里不再介绍。

因为变换矩阵的标准格式为：
$$T_{imu}^{camera}=\left[\begin{array}{ccc}{R}_{imu}^{camera}& ,& t_{imu}^{camera}\\ & & \\ 0^T& ,& 1\end{array}\right]$$
所以：

$$R_{imu}^{camera}=R_{front}^{camera}\ast R_{car}^{front}\ast R_{imu}^{car}\text{\hspace{1em}(3)}$$

$$t_{imu}^{camera}=R_{front}^{camera}\ast R_{car}^{front}\ast (t_{imu}^{car}-t_{front}^{car})\text{\hspace{1em}(4)}$$
公式(4)并不直接是相机外参文件中的偏移向量，公式(4)表示imu相对于camera的偏移向量，所以需要把相机外参文件中camera相对于imu的偏移向量$(x_{外参},y_{外参},z_{外参})$转化为公式(4),变为了$(-x_{外参},z_{外参},-y_{外参})$。

到这里，我们就只需要考虑上面公式(4)的 $t_{imu}^{car}-t_{front}^{car}$，通过画图：

可知$t_{imu}^{car}-t_{front}^{car}$是在car坐标系下从front相机坐标系原点指向imu坐标系原点的向量，就是imu坐标系原点相对于front相机坐标系原点在car坐标系下偏移，只不过这个偏移向量的数值不是在imu坐标系下的向量数值，而是在car坐标系下。

代码中是把相机外参文件中$(x,y,z)$表示的偏移向量$t_{front}^{imu}$，在没有考虑imu2car偏移向量的情况下，直接转化为了公式(1)中歪camera2car变换矩阵$T_{歪camera}^{car}$中的平移向量$t_{front}^{car}$， 也就是说，把imu坐标系下的平移向量$t_{front}^{imu}=\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]$转化到车身坐标系下，不考虑imu2car偏移，就变成了 $t_{front}^{car}=\left[\begin{array}{c}{y}_0\\ -x_0\\ z_0\end{array}\right]$。

这么做是默认imu和car坐标系原点重合，所以这么做是有问题的，当imu和car之间有偏移的时候，就会出错，只是目前所有车的imu和car原点都重合，所以不影响。以后有空需要把代码中这个bug补上。

下面这个图别看，是我以前的错误想法，留着让我自己更清晰。

---

(2)相机障碍物后处理时(camera_process)，把车身坐标系的激光点云障碍物转化到歪相机坐标系：

相机后处理模块的输入是车身坐标系的激光点云障碍物。
$$\begin{array}{rl}\left[\begin{array}{c}{P}_{car}\\ \\ 1\end{array}\right]& =T_{imu}^{car}\ast T_{camera}^{imu}\ast \left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]\\ & \\ & =\left[\begin{array}{cc}{R}_{imu}^{car}& t_{imu}^{car}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{cc}{R}_{front}^{imu}\ast R_{camera}^{front}& t_{front}^{imu}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]\\ & \\ & =\left[\begin{array}{cc}{R}_{imu}^{car}\ast R_{front}^{imu}\ast R_{camera}^{front}& R_{imu}^{car}\ast t_{front}^{imu}+t_{imu}^{car}\\ & \\ 0^T& 1\end{array}\right]\ast \left[\begin{array}{c}{P}_{camera}\\ \\ 1\end{array}\right]\end{array}$$

得到从歪camera坐标系点到车辆坐标系点的变换矩阵就是：
$$T_{camera}^{car}=\left[\begin{array}{cc}{R}_{imu}^{car}\ast R_{front}^{imu}\ast R_{camera}^{front}& R_{imu}^{car}\ast t_{front}^{imu}+t_{imu}^{car}\\ & \\ 0^T& 1\end{array}\right]\text{\hspace{1em}(3)}$$
然后再对$T_{camera}^{car}$求逆，即得到把车身坐标系的激光点云变换到歪相机坐标系的变换矩阵 $T_{car}^{camera}$。

---

疑问： 我们的目的是求车身坐标系某点投影到歪相机坐标系的变换矩阵car2camera，为什么要通过求camera2car的逆来间接求变换矩阵car2camera，而不直接求车身坐标系某点投影到歪相机坐标系的变换矩阵car2camera呢？

答： 因为从pixel坐标系点映射到车辆坐标系的公式中会用到的平移向量，例如相机相对与imu坐标系的平移向量，imu2car的偏移向量，都是直接从相机配置文件和imu2car.yaml中读取的，而这些参数都是从歪camera坐标系点到car坐标系点直接使用的。你如果不直接使用他们，而是做相应的反变换(从车辆坐标系映射到pixel坐标系点)也可以，但是总得选择一个方向，而直接使用标定得到的参数是最方便的，旋转矩阵是可以直接反变换，但是平移向量就并非直接相加减这么简单就是反变换，平移向量会涉及到乘法(自己推导一下公式就明白了)，所以通过先求camera2car，再求它的逆来间接求变换矩阵car2camera。

---

(3) 像素坐标系某2D点反投影到3D车身坐标系时：

这里是利用已经标定好的相机内外参。
下面的 $z_c$ 是车身坐标某点在歪相机坐标系的 $z$ 坐标，方向向前(但不是前向相机坐标系的正前方，它是歪的)。
从像素坐标系的某点反投影到车身坐标系的地面某点的公式，可参考我另一篇博客【自动驾驶】像素坐标系2D -＞ 3D车身坐标系 (附代码)，也可以自己推导，如下：
$$P_{car}=z_c\ast R_{front}^{car}\ast R_{歪camera}^{front}\ast K^{-1}\ast P_{u,v}+R_{imu}^{car}\ast t_{front}^{imu}+t_{imu}^{car}=\left[\begin{array}{c}{x}_{car}\\ y_{car}\\ z_{car}\end{array}\right]$$
所以只有三个值是未知数：$z_c、x_{car}、y_{car}$，可通过下面方法解出这三个值：

• 1.通过地面上的某点在车身坐标系的高度值$z_{car}$已知，对公式(10)的第三维度的等式求解，求出在地面上的某点P在相机坐标系下的深度值 $z_c$；
• 2.然后就可以利用$z_c$，根据公式(10)求解出$(x_{car},y_{car})$，这样就得到了像素坐标系的某2D像素点$P(u,v)$反投影到3D车身坐标系下的点$(x_{car},y_{car},z_{car})$。

---

标定过程与像素坐标系某2D点反投影到3D车身坐标系的异同点：

无论是标定时，还是像素坐标系某2D点反投影到3D车身坐标系时，都是先得到camera2car的变换矩阵。不同的是， 像素坐标系某2D点反投影到3D车身坐标系时是直接使用camera2car，而标定时需要对camera2car变换矩阵求逆得到car2camera。

---

特别需要注意的是：

从车身坐标系到相机坐标系的平移向量，与从相机坐标系到车身坐标系的平移向量的数值并不是相反数。
因为涉及到旋转矩阵的逆运算，这里是重中之重。
由于camera是标定到imu坐标系(和车身坐标系原点重合)中，所以相机外参配置文件中的平移向量是，这个camera在imu坐标系下的坐标， 并不是imu在camera坐标系的坐标。

如果是从车身坐标系转化到camera坐标系，就也直接使用配置文件的偏移向量，只不过后面需要对求逆。

---

带平移向量的变换矩阵求逆公式推导:

从点$P$经过变换矩阵$T$得到$P^{\prime }$点:
$$P^{\prime }=T\ast P=\left[\begin{array}{cc}R& \vec{t}\\ 0^T& 1\end{array}\right]\ast P$$
求逆变换：
$$P=T^{-1}\ast P^{\prime }=\left[\begin{array}{cc}{R}^{-1}& -R^{-1}\vec{t}\\ 0^T& 1\end{array}\right]\ast P^{\prime }$$
由于旋转矩阵都是正交矩阵，所以 $R^{-1}=R^T$，因此，上面逆变换可以写成这样：

$$P=T^{-1}\ast P^{\prime }=\left[\begin{array}{cc}{R}^T& -R^T\vec{t}\\ 0^T& 1\end{array}\right]\ast P^{\prime }$$
逆变换矩阵: $$T^{-1}=\left[\begin{array}{cc}{R}^T& -R^T\vec{t}\\ 0^T& 1\end{array}\right]$$