激光雷达和相机的坐标转换以及联合标定
- 1.坐标系说明
- 2.坐标系转换
- 3.基于autoware功能包的联合标定
1.坐标系说明
1）Lidar坐标系
激光雷达坐标系可以描述物体与激光雷达的相对位置，表示为[XL, YL, ZL]，其中原点为激光雷达几何中心，XL轴水平向前，YL轴水平向左，ZL轴竖直向上，符合右手坐标系规则。
2）相机坐标系
相机坐标系可以描述物体与相机的相对位置，表示为[XC, YC, ZC]，其中原点为相机光心 O 点，XC 轴与 x 轴平行，YC 轴与 y轴平行，ZC轴与摄像机光轴平行，与图像平面垂直。
3）图像坐标系
图像坐标系指在图像像素坐标系下建立以物理单位(如毫米)表示的坐标系，使像素尺度具有物理意义，表示为[x, y]，其中原点为相机主点，即相机光轴与图像平面的交点，一般位于图像平面中心，x 轴与 u 轴平行，y 轴与 v 轴平行；
4）像素坐标系
像素坐标系表示为[u, v]，其中原点为图像左上角，u 轴水平向右，v 轴竖直向下；
2.坐标系转换
1）世界坐标系到相机坐标系的转换
世界坐标系和相机坐标系同处于三维空间下，主要是坐标原点与各个轴的方向不同，所以进行刚体变换就ok了（R为3x3旋转矩阵，T为3x1平移矩阵）
2）相机坐标系到图像坐标系的转换
相机坐标系与图像坐标系为透视投影的关系。
f为相机的焦距，相机坐标系下点p与图像坐标系下投影点p(x,y)的坐标关系为：
最终的转换关系为：
3）图像坐标系与像素坐标系的转换
图像坐标系与像素坐标系为平移转换关系，o1->像素坐标系原点，o->图像坐标系原点 ；示意图如下：
点o在像素坐标系下的坐标为(Uo,Vo),变换公式为：
式中 dx 和 dy 分别为单个像素在像平面的长和宽，转换为矩阵：
最后，激光雷达和相机的坐标转换关系为：
3.基于autoware功能包的联合标定
autoware的安装很是烦人，网上又说有独立出来的calibration toolkit，但是在安装编译的时候也总是报错，最后找到了autoware的功能包，标定过程做个记录。
1）功能包下载编译
在AUTOWARE_AI 下找到uilits中的autoware_camera_lidar_calibrator包，根据launch文件要求下载其他的功能包，并放置在一个工作空间下编译。主要是以下三个需要在autoware其他文件夹中找到。其中image_view2 用sudo apt-get install ros-melodic - image_view2 安装即可
<node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher" ns="$(arg camera_id)">
<node pkg="image_processor" type="image_rectifier" name="rectifier" ns="$(arg camera_id)" output="screen">
<node pkg="image_view2" type="image_view2" name="image_view2" ns="$(arg camera_id)" output="screen">
<remap from="image" to="image_rectified" />2）相机内参标定
运行命令：
rosrun autoware_camera_lidar_calibrator cameracalibrator.py --square 0.108 --size 8x6 image:=/camera/image_raw将image的topic换成你自己的，当出现calibrate后点击，过一会儿再点save就会保存一个yaml文件；
3）联合标定
roslaunch autoware_camera_lidar_calibrator camera_lidar_calibration.launch intrinsics_file:=/home/zqh/***.yaml image_src:=/camera/image_raw忘记拍照了，大致流程，需要找一个人站在摄像头范围内移动9次；每移动一次首先点击一下image_rectify上面的人的位置，然后在rviz中的对应人的点云上点publish_point（选择点云时最好选择拐点或者是交叉点）。
重复9次之后，会在home下生成标定文件。
