Autoware标定激光雷达和相机记录

以Rfans-16为例，简单记录一下自己的学习标定之路，注意：如果激光雷达不是X轴朝前，需要对点云做一下旋转变换。

0. 连接相机和激光雷达，激光雷达X轴向前，Y轴向左，Z向上，相机默认正向前方。建立ROS工作空间

$ mkdir -p catkin_lidar_camera/src
$ catkin_make

1. 首先下载相机驱动，可以在github上找usb_cam https://github.com/ros-drivers/usb_cam将usb_cam 包放到src下，同将激光雷达的包放到src下。

2. 在catkin_lidar_camera目录下打开终端

$ source devel/setup.bash
$ catkin_make 编译项目。

3. 修改usb_cam-test.launch 中相机的端口号，启动usb相机

$ roslaunch usb_cam usb_cam-test.launch

4. 启动rfans激光雷达

$ roslaunch rfans_driver node_manager.launch

5. 下载autoware，在～/autoware/ros目录下打开终端

$ catkin_make 

编译项目需要一段时间，期间最好不要运行其他程序（注意cuda和驱动等）

6. 编译成功后继续执行

$ ./run

出现Autoware的主界面，证明编译成功。

 

7. 点击界面rviz打开，读取rfans的rviz配置文件（在rfans驱动包），在文件栏添加panels，添加ImageViewerPlugin

(NOTE: 或者进入autoware根目录终端后，source devel/setup.bash，再打开rviz，直接终端打开rviz找不到这个插件）

8. 标定相机参数，这部分就是ros里面的相机标定，标定后文件在home文件夹下，square 是标定板的大小，单位米，image:后面维图像的话题。

rosrun autoware_camera_lidar_calibrator cameracalibrator.py --square 0.04 --size 8x8 image:/usb_cam/image_raw

%YAML:1.0
---
CameraExtrinsicMat: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1., 0., 0., 0., 0., 1., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
       1. ]
CameraMat: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 3.8450357054617172e+02, 0., 3.4754519060153473e+02, 0.,
       3.8333442838042225e+02, 2.6041442152424389e+02, 0., 0., 1. ]
DistCoeff: !!opencv-matrix
   rows: 5
   cols: 1
   dt: d
   data: [ -1.1129042524997940e-02, -8.0253952291279903e-03,
       -3.2209335857971981e-04, 1.3874403093426296e-03,
       2.9361254263442723e-03 ]
ImageSize: [640, 480]
Reprojection Error: 0.0
DistModel: plumb_bob

9. 重新打开一个终端，启动标定程序

$ roslaunch autoware_camera_lidar_calibrator camera_lidar_calibration.launch intrinsics_file:=/home/sun/20200530_1126_autoware_camera_calibration.yaml image_src:=/usb_cam/image_raw

会出现一个畸变矫正后的图像。

10. 开始标定：选择图像中和点云中匹配的点对

在图像中点一下，会出现一个小红点，在rviz的点云中选择对应的点，先点击publish，再点击对应的三维点，在程序终端会出现选择的点的结果。（注意选对应点的时候，为了尽可能保证正确匹配，尽量选择点云中的拐角或者交叉点，另外，将点云旋转到一个合适的位置，尽量保证你选择的点没有其他太多不同深度的点，以免出现误点，影响标定的效果）。

11. 连续选择9对匹配点后，观察终端，在/home下会生成标定文件。

%YAML:1.0
---
CameraExtrinsicMat: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 8.5796696358197755e-02, 2.7875146135983625e-01,
       -9.5652315689886724e-01, 1.1717870086431503e-01,
       9.5397844721965463e-01, 2.5390066317902082e-01,
       1.5956056993389645e-01, 1.1259393692016602e+00,
       2.8733960592719487e-01, -9.2619224571738301e-01,
       -2.4413904816435067e-01, 7.4070060253143311e-01, 0., 0., 0., 1. ]
CameraMat: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 3.8450357054617172e+02, 0., 3.4754519060153473e+02, 0.,
       3.8333442838042225e+02, 2.6041442152424389e+02, 0., 0., 1. ]
DistCoeff: !!opencv-matrix
   rows: 1
   cols: 5
   dt: d
   data: [ -1.1129042524997940e-02, -8.0253952291279903e-03,
       -3.2209335857971981e-04, 1.3874403093426296e-03,
       2.9361254263442723e-03 ]
ImageSize: [ 640, 480 ]
ReprojectionError: 0
DistModel: plumb_bob

12. 查看标定效果

这一步花了我一会时间摸索，首先是因为自己打开rviz，始终找不到ImageViewerPlugin这个插件，另外一个是因为，在终端执行rosrun calibration_publisher calibration_publisher calibration_file:=/home/sun/20200530_lidar_camera_calibration.yaml时，一直提示错误，无奈写了个launch文件（如下），更改frame，设置发布坐标变换、相机内参，以及标定的外参矩阵，就OK了。

$ roslaunch calibration_publisher calibration_publisher.launch

<launch>
    <arg name="image_src" default="/usb_cam/image_raw" />
    <arg name="camera_info_src" default="/camera_info" />
    <arg name="camera_id" default="/" />
    <arg name="intrinsics_file" default="/home/sun/20200530_173120_autoware_lidar_camera_calibration.yaml"/>
    <arg name="compressed_stream" default="false" />
    <arg name="target_frame" default="rfans16"/>
    <arg name="camera_frame" default="camera"/>

    <node if="$(arg compressed_stream)" name="decompress" type="republish" pkg="image_transport" output="screen" args="compressed in:=/$(arg camera_id)/$(arg image_src) raw out:=/$(arg camera_id)/$(arg image_src)" />

    <node pkg="calibration_publisher" type="calibration_publisher" name="calibration_publisher" ns="$(arg camera_id)">
        <param name="register_lidar2camera_tf" type="bool" value="true"/>
        <param name="publish_extrinsic_mat" type="bool" value="true"/>
        <param name="publish_camera_info" type="bool" value="true"/>
        <param name="image_topic_src" value="$(arg image_src)"/>
        <param name="calibration_file" value="$(arg intrinsics_file)"/>
        <param name="target_frame" type="str" value="$(arg target_frame)"/>
        <param name="camera_frame" type="str" value="$(arg camera_frame)"/>
    </node>

</launch>

启动点云到图像的转换节点，修改点云话题为rfans的

$ rosrun points2image points2image _points_node:=/rfans_driver/rfans_points

13. rviz查看标定结果

rfans发布的点云话题是在world坐标系下 ，标定程序发布的坐标变换是相机到激光雷达。camera—rfans16（在上面calibration_publisher的launch 文件中设置）但是没有world到rfans16的坐标变换，无法查看tf关系，所以，发布一个world-rfans16的静态tf变换

$ rosrun tf static_transform_publisher 0 0 0 0 0 0 world rfans16 100

14. 在ImageViewerPlugin 中 选择图像话题，以及点云话题： /points_image，即可查看最终的标定效果