【多传感器融合】激光雷达与相机前融合

坐标变换的方法采用了 Geiger, A 等在论文中提到的方法，最终将激光点云转换到相机坐标系下。毫米波雷达目标变换也是一样的。
要将 Velodyne 坐标中的点 x 投影到左侧的彩色图像中 y：

将 Velodyne 坐标中的点投影到右侧的彩色图像中：

是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中，再修正，
是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中，再修正，然后投影到编号为 2 的相机（左彩色相机）。
修正后的空间三维点云Tx (x, y, z,1) 投影到第 i个摄像机图像中的点Ty (u,v,1) 的投影为：

而将参考相机坐标中的 3D 点 x 投影到第 i 个图像平面上的点 y,还需要考虑校正旋转矩阵如下式：

扩展为 4×4 矩阵。最后,如果要把激光雷达坐标中的点转换到参考相机坐标上：


图 3-3、图 3-4、图 3-5 为激光点云坐标系转换至摄像头的坐标系中，可以看出数据对齐的比较好，能够满足后期融合的要求。