【自动驾驶】31.相机外参标定过程与2D点反投影到3D的过程对比

这里说一下，我们项目定义的imu坐标系和车身坐标系的原点重合。
我项目中标定的平移向量的x,y,z是标定到imu坐标系。
前向相机坐标系是一个标准坐标系，表示相机位姿的三个欧拉角都为0，X右正，Y下正，Z前正。
以下是相机外参配置文件中的平移向量的值：

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) 标定时：

标定时，是3D激光点云投影到2D像素平面，我们通过读取相机外参配置文件中的欧拉角值和平移向量，得到外参初值，然后手动调节这六个外参，使得激光点云投影到像素平面，调参直到障碍物的点云和像素平面的对应障碍物完美重合，就得到了标定后的相机外参。

具体实现细节是：

• 1.由于相机位姿欧拉角是相机相对于前向相机坐标系的姿态角，平移向量是相机在imu坐标系下的坐标，所以可以很容易地根据配置文件或手动调节后外参欧拉角得到歪相机的某点投影到前向相机的旋转矩阵camera2front；
• 2.再根据前向相机坐标系到car坐标系的坐标系变换得到front2car。这里得到的车身坐标系并非真正的车身坐标系，而是原点和前向相机坐标系重合的车身坐标系。
• 3.再利用相机外参的平移向量对上一步得到的点进行平移，就得到了车身坐标系的变换矩阵。在代码中，是在R=front2car * camera2front后，然后把旋转矩阵R与平移向量整合成一个4x4齐次变换矩阵camera_to_car_augm，然后对这个齐次矩阵求逆，就得到了车身坐标系某点投影到歪相机坐标系的变换矩阵car2camera_ = camera_to_car_augm.inverse();
• 4.再将上一步得到的歪相机坐标系下的点通过相机内参矩阵进行变换，(以及畸变处理)，就可以把该歪相机坐标系的点投影到像素坐标系中.

综上，就是利用这6个相机外参先得到camera坐标系的点 投影变换到car坐标系的变换矩阵，然后再对这个变换矩阵求逆，就得到了car2camera，得到激光点云投影到歪相机坐标系的点之后，再通过相机内参矩阵，就可以把该点投影到像素坐标系中了。

---

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

这里是利用已经标定好的相机内外参。
下面的Zc是车身坐标某点在歪相机坐标系的Z坐标，方向向前(但不是前向相机坐标系的正前方，它是歪的)。

• 1.从像素坐标系的2D点P0，通过相机内参矩阵的逆,得到歪相机坐标系点(Xc,Yc,Zc)/Zc。
  Eigen::Matrix<float, 3, 1> org_camera_point = camera_intrinsic_inverse_ * img_vector;
• 2.把上一步得到的歪相机坐标系点(Xc,Yc,Zc)/Zc用欧拉角旋转矩阵进行变换，得到前向相机坐标系点/Zc，即(Xc',Yc',Zc')/Zc,但是后面只用得到Yc'/Zc。
• 3.计算缩放因子scale，scale = camera2ground_height_ / rotate_point(1, 0);，可以化简为scale=Zc*camera2ground_height_/某点在前向相机坐标点的Yc'.
• 4.计算P0投影到歪相机坐标系下的坐标：
  Eigen::Matrix<float, 3, 1> camera_point = org_camera_point * scale;
  上面的camera_point是P0投影到歪相机坐标系下的坐标原因：
  上面等号右边代入展开camera_point=(歪相机坐标系点(Xc,Yc,Zc)/Zc)*Zc*camera2ground_height_/点P0投影到前向相机坐标点的Yc'；
  化简得：camera_point=camera2ground_height_*歪相机坐标系点(Xc,Yc,Zc)/点P0投影到前向相机坐标点的Yc'
  由于车道线或可行驶区域点是地面上的点，所以Yc'=camera2ground_height_， 因此：camera_point= 歪相机坐标系某点(Xc,Yc,Zc)。
• 5.下面的car_vector是二维像素点P0投影到(原点和前向相机原点重合的)车身坐标系的坐标：
  Eigen::Matrix<float, 3, 1> car_vector = camera2car_ * rotate_matrix_ * camera_point;
• 6.最终的P0投影到车身坐标系的3D点car_point为：

car_point->x = car_vector(0, 0) + camera_coefficient_->camera_extrinsic(0, 3);
car_point->y = car_vector(1, 0) + camera_coefficient_->camera_extrinsic(1, 3);
// 虽然在3d俯视图看不出高度有什么影响，但是如果z+1000就会看出衔接处明显中断的痕迹,为什么？
// car_point->z = car_vector(2, 0);  // -2.3749
car_point->z = car_vector(2, 0) + camera_coefficient_->camera_extrinsic(2, 3);

以Z坐标来分析，上面的运算不是减去偏移却是加上的原因：因为车身坐标系的z轴向上为正，imu在地面以上1.8m的位置，地面上的点z值必定是负数，所以是加上z偏移，而不是减去。

为什么得到的结果car_vector(2, 0)恰好是-1*camera2ground_height_？这么准？
答：因为前面使用了camera2ground_height_，就是取地面上的点来处理的。
所以地面上的二维像素点投影到歪相机坐标系某点，再经过欧拉角旋转矩阵和坐标系变换，
得到了地面上二维像素点投影到(原点和前向相机原点重合的)车身坐标系的坐标，Z坐标自然就是camera2ground_height_的相反数。

---

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

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

---

特别需要注意的是：

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

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