激光雷达坐标系、相机坐标系与到图像坐标系之间的转换
本文解决的问题是经联合标定后，点云和图像之间坐标相互转换的问题。需要的先验知识和条件：相机标定；激光雷达和相机联合标定；相机内参、相机和雷达外参。
把雷达坐标系看成世界坐标系，则世界坐标系中任意一点W，其世界坐标为：
其世界齐次坐标为：
又假设雷达坐标系到相机坐标系的转换矩阵为E，（该矩阵即是外参矩阵，取其英文Extrinsic首字母E），假设其逆矩阵为：
设点W在相机坐标系下的坐标表示为：
其在相机坐标系下的齐次坐标为：
则有：
即：
为消除齐次坐标、简化计算，特令：
则公式变为：
设相机坐标系到图像坐标系的转换矩阵K（K矩阵即是相机内参矩阵）及其逆矩阵K^(-1)为：
设点W在图像坐标系下的坐标为：
其在图像坐标系下的齐次坐标为：
则有：
即有：
带入上式即可得相机坐标Ximg_h和雷达坐标的互相转换公式：
其中，
代表对矩阵M取前三行前三列。
由上式可知：已知世界（雷达）坐标，可以将其精确地投影到图像坐标上。但要使相机坐标能准确转换到雷达坐标，必须知道Zcam，这是点离相机平面的距离，由于仅从图像中无法得知Zcam，所以仅用图像无法把图像上的点投影到世界（雷达）坐标系。