激光雷达坐标系、相机坐标系与到图像坐标系之间的转换

激光雷达坐标系、相机坐标系与到图像坐标系之间的转换
本文解决的问题是经联合标定后，点云和图像之间坐标相互转换的问题。需要的先验知识和条件：相机标定；激光雷达和相机联合标定；相机内参、相机和雷达外参。

把雷达坐标系看成世界坐标系，则世界坐标系中任意一点W，其世界坐标为：

其世界齐次坐标为：

又假设雷达坐标系到相机坐标系的转换矩阵为E，（该矩阵即是外参矩阵，取其英文Extrinsic首字母E），假设其逆矩阵为：

设点W在相机坐标系下的坐标表示为：

其在相机坐标系下的齐次坐标为：

则有：

即：

为消除齐次坐标、简化计算，特令：

则公式变为：

设相机坐标系到图像坐标系的转换矩阵K（K矩阵即是相机内参矩阵）及其逆矩阵K^(-1)为：

设点W在图像坐标系下的坐标为：

其在图像坐标系下的齐次坐标为：

则有：

即有：

带入上式即可得相机坐标Ximg_h和雷达坐标的互相转换公式：

其中，代表对矩阵M取前三行前三列。

由上式可知：已知世界（雷达）坐标，可以将其精确地投影到图像坐标上。但要使相机坐标能准确转换到雷达坐标，必须知道Zcam，这是点离相机平面的距离，由于仅从图像中无法得知Zcam，所以仅用图像无法把图像上的点投影到世界（雷达）坐标系。