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

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

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

我项目中标定的平移向量的 x,y,z\color{red}x,y,zx,y,z 是标定到imu坐标系。

以下是相机外参配置文件中的平移向量的值：

translation:# x是该相机在imu坐标系下，距离imu中心的横向距离，由于该相机在imu右边，所以是正数，1.08mx: 1.0800000429153442e+00# y是该相机在imu坐标系下，距离imu中心的纵向距离，由于该相机在imu后面，所以是负数，-1my: -1.0290000438690186e+00# z是该相机在imu坐标系下，距离imu中心的高度距离，由于该相机在imu下面，所以是负数，-0.070m即7cmz: -7.0000000298023224e-02

`(1) 标定时：`

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

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

相机标定模块的输入是imu坐标系的激光点云，经过了imu2car转换到车身坐标系，此时的imu2car平移向量是被设定为0,即imu坐标系和car坐标系只有旋转，没有平移，原点重合。
[Pcar1]=Tfrontcar∗Tcamerafront∗[Pcamera1]=[Rfrontcartfrontcar0T1]∗[Rcamerafronttcamerafront0T1]∗[Pcamera1]\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⎦⎤
由于tcamerafront=0⃗3∗1\color{red}t^{front}_{camera}=\vec{0}_{3*1}tcamerafront=03∗1,所以从歪camera坐标系点到车辆坐标系点的变换矩阵就是：
Tcameracar=[Rfrontcartfrontcar0T1]∗[Rcamerafront0⃗0T1]=[Rfrontcar∗Rcamerafronttfrontcar0T1](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)
然后再对Tcameracar\color{red}T_{camera}^{car}Tcameracar求逆，即得到把车身坐标系的激光点云变换到歪相机坐标系的变换矩阵 Tcarcamera\color{red}T_{car}^{camera}Tcarcamera。

tfrontimu\color{red}t^{imu}_{front}tfrontimu是front相机坐标系原点相对imu坐标系原点的偏移向量，而tfrontcar\color{red}t^{car}_{front}tfrontcar是front相机坐标系原点相对car坐标系原点的偏移向量。
本模块中直接把 tfrontimu\color{red}t^{imu}_{front}tfrontimu调换x、y方向和正负得到 tfrontcar\color{red}t^{car}_{front}tfrontcar，这样做就是设定了imu坐标系原点和car坐标系原点重合，即 timucar=0⃗\color{red}t^{car}_{imu}=\vec{0}timucar=0。

下面是imu坐标系下的激光点云的某点，转换到车辆坐标系下的公式：
[Pcar1]=[Rimucartimucar0T1]∗[Pimu1]\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坐标系点到车辆坐标系点下的公式为：
[Pcar1]=[Rfrontcar∗Rcamerafronttfrontcar0T1]∗[Pcamera1]\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坐标系下的公式：
[Pcamera1]=[Rfrontcamera∗Rcarfront∗RimucarRfrontcamera∗Rcarfront∗(timucar−tfrontcar)0T1]∗[Pimu1]\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坐标系点的变换矩阵：
Timucamera=[Rfrontcamera∗Rcarfront∗Rimucar,Rfrontcamera∗Rcarfront∗(timucar−tfrontcar)0T,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)的变换矩阵 Timucamera\color{red}T_{imu}^{camera}Timucamera，就是把imu坐标系下的激光点云转化到歪相机坐标系点的变换矩阵。

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

因为变换矩阵的标准格式为：
Timucamera=[Rimucamera,timucamera0T,1]\color{red} T_{imu}^{camera} =\begin{bmatrix} R^{camera}_{imu} &,&t^{camera}_{imu}\\ \\0^T&,&1\end{bmatrix} Timucamera=⎣⎡Rimucamera0T,,timucamera1⎦⎤
所以：

Rimucamera=Rfrontcamera∗Rcarfront∗Rimucar(3)\color{red} R^{camera}_{imu} =R^{camera}_{front}*R^{front}_{car}*R^{car}_{imu} \tag{3} Rimucamera=Rfrontcamera∗Rcarfront∗Rimucar(3)

timucamera=Rfrontcamera∗Rcarfront∗(timucar−tfrontcar)(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)的 timucar−tfrontcar\color{red}t_{imu}^{car}-t^{car}_{front}timucar−tfrontcar，通过画图：

可知timucar−tfrontcar\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)表示的偏移向量tfrontimu\color{red}t_{front}^{imu}tfrontimu，在没有考虑imu2car偏移向量的情况下，直接转化为了公式(1)中歪camera2car变换矩阵T歪cameracar\color{red}T_{歪camera}^{car}T歪cameracar中的平移向量tfrontcar\color{red}t^{car}_{front}tfrontcar，也就是说，把imu坐标系下的平移向量tfrontimu=[x0y0z0]\color{red}t_{front}^{imu}=\begin{bmatrix} x_0\\ y_0\\z_0\end{bmatrix}tfrontimu=⎣⎡x0y0z0⎦⎤转化到车身坐标系下，不考虑imu2car偏移，就变成了 tfrontcar=[y0−x0z0]\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)，把车身坐标系的激光点云障碍物转化到歪相机坐标系：`

相机后处理模块的输入是车身坐标系的激光点云障碍物。
[Pcar1]=Timucar∗Tcameraimu∗[Pcamera1]=[Rimucartimucar0T1]∗[Rfrontimu∗Rcamerafronttfrontimu0T1]∗[Pcamera1]=[Rimucar∗Rfrontimu∗RcamerafrontRimucar∗tfrontimu+timucar0T1]∗[Pcamera1]\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坐标系点到车辆坐标系点的变换矩阵就是：
Tcameracar=[Rimucar∗Rfrontimu∗RcamerafrontRimucar∗tfrontimu+timucar0T1](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)
然后再对Tcameracar\color{red}T_{camera}^{car}Tcameracar求逆，即得到把车身坐标系的激光点云变换到歪相机坐标系的变换矩阵 Tcarcamera\color{red}T_{car}^{camera}Tcarcamera。

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

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

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

这里是利用已经标定好的相机内外参。
下面的 zc\color{red}z_czc 是车身坐标某点在歪相机坐标系的 z\color{red}zz 坐标，方向向前(但不是前向相机坐标系的正前方，它是歪的)。
从像素坐标系的某点反投影到车身坐标系的地面某点的公式，可参考我另一篇博客【自动驾驶】像素坐标系2D -＞ 3D车身坐标系 (附代码)，也可以自己推导，如下：
Pcar=zc∗Rfrontcar∗R歪camerafront∗K−1∗Pu,v+Rimucar∗tfrontimu+timucar=[xcarycarzcar]\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⎦⎤
所以只有三个值是未知数：zc、xcar、ycar\color{red}z_c、x_{car}、y_{car}zc、xcar、ycar，可通过下面方法解出这三个值：

1.通过地面上的某点在车身坐标系的高度值zcar\color{red}z_{car}zcar已知，对公式(10)的第三维度的等式求解，求出在地面上的某点P在相机坐标系下的深度值 zc\color{red}z_czc；
2.然后就可以利用zc\color{red}z_czc，根据公式(10)求解出(xcar,ycar)\color{red}(x_{car},y_{car})(xcar,ycar)，这样就得到了像素坐标系的某2D像素点P(u,v)P(u,v)P(u,v)反投影到3D车身坐标系下的点(xcar,ycar,zcar)\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坐标系，就也直接使用配置文件的偏移向量，只不过后面需要对求逆。

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

从点PPP经过变换矩阵TTT得到P′P'P′点:
P′=T∗P=[Rt⃗0T1]∗PP'=T*P=\begin{bmatrix} R&\vec{t} \ \\ 0^T&1\end{bmatrix}*PP′=T∗P=[R0Tt 1]∗P
求逆变换：
P=T−1∗P′=[R−1−R−1t⃗0T1]∗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=RTR^{-1}=R^TR−1=RT，因此，上面逆变换可以写成这样：

P=T−1∗P′=[RT−RTt⃗0T1]∗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=[RT−RTt⃗0T1]T^{-1}=\begin{bmatrix} R^T&-R^T\vec{t} \ \\ 0^T&1\end{bmatrix}T−1=[RT0T−RTt 1]