单目摄像头的内外参标定

任务动机：基于ORB-SLAM3+单目广角摄像头进行二次开发，辅助激光导航实现机器人全局重定位。需要对单目摄像头进行内参和外参标定。

任务描述：
1. 镜头畸变和内参标定
用棋盘格标定

2. 摄像机与机器人坐标系间的位姿标定

• 在线数据采集：找一个场景内容丰富的环境 启动激光雷达SLAM，启动摄像机图像采集 录制ros bag（最好录制全部，如果空间不够，可以只选激光雷达，摄像机图像，tf） 控制机器人走几圈+走走八字
• 离线处理：播放bag，启动单目摄像机的ORB-SLAM 保存ORB-SLAM的位姿输出结果和激光雷达SLAM的位姿输出序列（都要有时间戳）
• 标定：提取相同时刻的ORB-SLAM和激光雷达SLAM的位姿对 计算二者间的位姿关系

1. 引言

        标定就是已知标定控制点的世界坐标和像素坐标， 去解算映射关系。一旦这个关系解算出来了，就可以由点的像素坐标去反推它的世界坐标，有了这个世界坐标，就可以进行测量等其他后续操作了。在图像测量过程以及机器视觉应用中，为确定空间物体表面某点的三维几何位置与其在图像中对应点之间的相互关系，必须建立相机成像的几何模型，这些几何模型参数就是相机参数。在大多数条件下这些参数必须通过实验与计算才能得到，这个求解参数的过程就称之为相机标定。

        相机需要标定的参数通常分为内参和外参两部分。外参确定了相机在某个三维空间中的位置和朝向，至于内参，可以说是相机内部的参数。外参数矩阵可以告诉你现实世界点(世界坐标)是怎样经过旋转和平移，然后落到另一个现实世界点(摄像机坐标)上。内参数矩阵告诉你上述那个点在外参矩阵的基础上，是如何继续经过摄像机的镜头、并通过针孔成像和电子转化而成为像素点的。而畸变矩阵则告诉你为什么上面那个像素点并没有落在理论计算该落在的位置上，还产生了一定的偏移和变形.

        本文介绍如何使用Autoware对摄像头进行内参标定，再通过ORB-SLAM对相机和雷达进行联合标定。

2. 镜头畸变与内参标定

        ROS官方提供了一个ROS camera_calibration工具包，可以使用它进行内参标定，但是使用下来效果并不好，可能会出现无法保存参数的情况，更推荐使用Autoware，尤其是Autoware还自带了Calibration Toolkit（1.9版本之前，需要在GitHub里面找到之前的版本地址，新的版本取消了），对于激光雷达和相机的联合标定更加友好。Autoware camera_calibration 工具包 与ROS官方的并没有太大区别，可以看作是官方ROS标定工具的一个分支（即ROS Camera Calibration Tools）。

2.1 安装Autoware

        官方推荐使用Docker安装，但是为了快捷本文直接源码安装。用Docker的时候要装NVIDIA Docker（因为要用GPU），一牵扯显卡就可能会出问题。建议直接源码编译了一遍Autoware。基于官方的源码安装教程，本文使用的是Ubuntu 16.04 + Kinetic，所以选择了1.12版本，安装方式如下。如果版本不同，需要参考官方教程。

1）安装系统依赖

$ sudo apt-get update
$ sudo apt-get install -y python-catkin-pkg python-rosdep ros-$ROS_DISTRO-catkin gksu
$ sudo apt-get install -y python3-pip python3-colcon-common-extensions python3-setuptools python3-vcstool
$ pip3 install -U setuptools

2）创建工作空间

$ mkdir -p autoware.ai/src
$ cd autoware.ai

3）下载Autoware配置文件

$ wget -O autoware.ai.repos "https://raw.githubusercontent.com/Autoware-AI/autoware.ai/1.12.0/autoware.ai.repos"

4）下载Autoware

$ vcs import src < autoware.ai.repos

5）rosdep安装依赖

$ rosdep update
$ rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO （这一步好像会出问题 网上有解决方案）

6）编译工作空间

        因为没有安装n卡显卡驱动，所以选择了没有CUDA支持的命令。

$ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release

        可以在文件夹 /autoware.ai/build/ 下面看到 autoware_camera_lidar_calibrator 这个文件夹，这个就是我们要用来做标定的工具包，然后进入到这个文件夹里面的 devel，在devel里面 source 一下 setup.bash。这时候，在终端里输入 rosrun auto， 双击 Tab 键就应该能看到一下命令

$ rosrun autoware_camera_lidar_calibrator

2.2 安装摄像头和发布相关话题

        首先检查Ubuntu有没有识别到摄像头，可以在应用程序里面直接打开 Cheese Webcam Booth。这个应用程序是Ubuntu自带的照相机工具，如果显示 No device found，就说明摄像头没有识别成功，还需要查询摄像头的厂商，安装ROS上的驱动。当然，如果是使用双目摄像头的话，可能比较复杂，即使安装好了驱动，也不一定能通过这个程序查看到画面。因为我使用的是单目摄像头，现在大部分的摄像头也都已经是免驱的，插上就可以直接使用。

        确定摄像头已经在Ubuntu正确安装之后，关闭Cheese，在终端里输入以下命令来启动摄像头的节点。（注意要先启动 roscore）

$ rosrun usb_cam usb_cam_node

        可以通过命令rostopic list看到出现了 /usb_cam/... 等一系列topic

        运行 rosrun usb_cam usb_cam_node 之后，这样就确保了单目相机正在通过ROS发布图像，其中我们过会需要使用的是 /usb_cam/image_raw。usb_cam是ROS的一个功能包，如果提示没有找到或无法运行的话，可以参考官方的教程先来安装这个功能。

2.3 标定相机内参与畸变

        首先要准备一个已知尺寸的标定板，可以在这里下载。本文使用的是8X6，方块大小为108mm的棋盘标定板。由于标定过程使用的是棋盘内部的角点进行，所以实际上使用的是9X7的棋盘标定板 。

1）启动摄像

$ rosrun usb_cam usb_cam_node

2）启动cameracalibrator.py

$ rosrun autoware_camera_lidar_calibrator cameracalibrator.py --square SQUARE_SIZE --size MxN image:=/image_topic

        --square:棋盘格中的每个方格的边长大小，单位为m，实际为 0.108

        --size:棋盘格的尺寸是几乘几，注意是inner，也就是出去边长最外圈方格数-1，实际为是8X6。如果这个参数不正确，在标定步骤中会发现标定程序毫无反应

        image :发布图像的话题名，实际是 /usb_cam/iamge_raw

        修改过的命令如下，当然，使用不同的棋盘格，这些参数也需要改变

$ rosrun autoware_camera_lidar_calibrator cameracalibrator.py --square 0.108 --size 8x6 image:=/usb_cam/image_raw

        这里可能会出现打开之后没有相机图像显示的问题，可以在命令后面加上 camera:=/usb_cam --no-service-check,完整命令如下

$ rosrun autoware_camera_lidar_calibrator cameracalibrator.py --square 0.108 --size 8x6 image:=/usb_cam/image_raw camera:=/usb_cam --no-service-check

        在上述节点启动起来后，会弹出一个图像框，右边有几个按钮，通过晃动棋盘格使得右边的Calibration按钮变绿。把棋盘格拿到相机前，看到棋盘格上有了mark并且右上方出现了四个滑动条类似的东西，分别表示X、Y、尺度与俯仰，根据提示哪一个自由度完成度不足来移动棋盘格。

        等Calibration按钮变绿了之后就可以点击一下，命令行窗口会给出计算出的标定结果，这一步窗口可能会变暗，并出现假死状态，需要耐心等待很长一段时间。命令行窗口给出计算出的标定结果后，点击Save按钮即可保存成在home目录下的命名类似于20200811_0036_autoware_camera_calibration.yaml。

        打开刚保存的 yaml 文件，可以看到以下内容

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: [ 6.4132780167960482e+02, 0., 4.6321749736436732e+02, 0.,
       6.4662171134096195e+02, 2.6575705715600145e+02, 0., 0., 1. ]
DistCoeff: !!opencv-matrix
   rows: 5
   cols: 1
   dt: d
   data: [ -4.1541156375699345e-01, 2.2717711085658737e-01,
       -3.9660433213257182e-04, -3.9461694367074014e-03,
       -7.6117176968176561e-02 ]
ImageSize: [640, 480]
Reprojection Error: 0.0
DistModel: plumb_bob

        在这个文件里面，CameraMat: !! opencv-matrix 代表着相机内部的参数，也就是内参。内参是一个3×3的矩阵，形式如下:

A=⎡⎣⎢⎢⎢⎢⎢⎢⎢fx000fy0cxcy1⎤⎦⎥⎥⎥⎥⎥⎥⎥A=[fx0cx0fycy001]

        其中，cx和cy表示相机光轴在图像坐标系中的偏移量，以像素为单位。 fx和fy则表示相机的焦距。详解的介绍可以查看这里。DistCoeff: !! opencv-matrix 代表着相机的畸变矩阵，形式如下:

data=[k1,k2,p1,p2,(k3,k4,k5,k6)]data=[k1,k2,p1,p2,(k3,k4,k5,k6)]

        注意，括号里的 (k3, k4, k5, k6) 并非是必须存在的参数。k1,k2,k3 ... 是径向畸变系数，p1,p2是切向畸变系数。径向畸变发生在相机坐标系转图像物理坐标系的过程中。而切向畸变是发生在相机制作过程，其是由于感光元平面跟透镜不平行。详解的介绍可以查看这里。因此，根据上述矩阵经过转换之后，可以由之前的 yaml 得到下面的相机参数。

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 6.4132780167960482e+02
Camera.fy: 6.4662171134096195e+02
Camera.cx: 4.6321749736436732e+02
Camera.cy: 2.6575705715600145e+02

Camera.k1: -4.1541156375699345e-01
Camera.k2:  2.2717711085658737e-01
Camera.p1: -3.9660433213257182e-04
Camera.p2: -3.9461694367074014e-03
Camera.k3: -7.6117176968176561e-02

 该参数将在以后用于ORB SLAM 的外参标定使用。