单目摄像头外参标定

任务动机：标定单目摄像头外参，为导航使用ORB-SLAM视觉全局定位二次开发做准备。

任务描述：

1. 简介

        在之前的内参标定中，我们已经得到了相机内参的参数，于是便可以进行外参标定。本文所讲的外参标定就是摄像机与机器人坐标系间的位姿标定，因此我们需要用到激光雷达和相机来确定其位姿关系。标定的大致流程是在一个场景内容丰富的环境中，启动激光雷达SLAM，启动摄像机图像采集并控制机器人走几圈，形成一个闭环并对比单目摄像机的ORB-SLAM保存ORB-SLAM的位姿输出结果和激光雷达SLAM的位姿输出序列。于是便可以提取相同时刻的ORB-SLAM和激光雷达SLAM的位姿对，计算二者间的位姿关系来进行标定。

2. 环境搭建

（1）安装 ORB-SLAM2

（2）安装Cartographer

（3）安装自动保存轨迹脚本

        由于环境配置较为复杂，这里直接分享一个已经搭建好环境的虚拟机。该虚拟机是使用 VMware Workstation 15.5 Pro，其他版本应该也可以运行，详细配置会稍后完善。虚拟机文件可以在 这里下载，提取码为casx。下载之后的文件大约10G左右，解压之后大约 55G。在Vmware中选择打开虚拟机，并选择刚刚解压好的文件夹中的 Ubuntu 16.04.vmx 便可以直接启动。第一次打开的时候可能会弹出提示问你是否 移动/复制了虚拟的文件，选择复制即可。如果正常出现了 Ubuntu的登录界面，则说明启动成功，该系统的密钥是123。

3. 准备

        首先在终端里面启动 roscore。因为标定需要用到 激光雷达 和 摄像头，所以要把这两个硬件连接到电脑上。注意！不管有没有使用hub，建议拔掉所有usb外设，先插入激光雷达的数据线，这时虚拟机会提示 连接到本机 还是 Ubuntu 16.04，选择连接到 Ubuntu 16.04 即可。同时可以在 虚拟机中的上方的导航栏中： 虚拟机→可移动设备 里面查看当前电脑所连接的外设，并显示有哪些设备连接到了虚拟机中。连接到虚拟机的设备会显示 "断开连接 （连接到主机）"，这表明该设备已经连接到了虚拟机。连接激光雷达到虚拟机中，之后执行以下命令来启动激光雷达。（文中所使用的激光雷达是 镭神 LS01B，已经安装好所有驱动，如使用不同的雷达，需要先安装雷达的驱动，并验证这个雷达可以使用并能够发出 /laser 的 Topic，可能需要根据不同的设备修改 launch 文件）

3.1 配置串口，检查串口权限

ls -l /dev/ttyUSB*

        根据出现的ttyUSB*，赋予端口权限。

sudo chmod 777 /dev/ttyUSB0

3.2 启动激光雷达

roslaunch ls01b_v2 ls01v_v2.launch

        这里使用镭神LS01B激光雷达。如果出现以下提示信息并且激光雷达开始旋转，则说明启动成功。

        如果提示信息中出现error字样，并且激光雷达也没有旋转的话，则说明连接有问题，极大的可能出现在配置串口的过程中。通过不断的测试，得到的结论就是断开所有外设，先只插入激光雷达的数据线并赋予端口权限，确定激光雷达正常启动之后，再连接其他外设。也可以尝试重启系统的方法，说不定就能转了。启动激光雷达之后，连接摄像头，这次不用赋予端口权限，插上就能用。

 3.3 启动单目摄像头       

        在终端里输入以下命令来启动摄像头（注意，这里的摄像头和之前的内参标定使用的是用一个摄像头，所以畸变参数也一样，如果更换了摄像头，则需要重新进行标定）

rosrun usb_cam usb_cam_node

        如果一切正常，可以通过命令rostopic list看到出现了 /usb_cam/... 等一系列topic，如下图所示

        运行 rosrun usb_cam usb_cam_node 之后，这样就确保了单目相机正在通过ROS发布图像，ORB-SLAM 就可以读取摄像头的内容。

4. 标定

4. 1 录制激光雷达数据

        标定时，需要同时记录ORB-SLAM 通过摄像头录制的kitti数据集和激光雷达记录的数据，这里可以在终端里先输入

rosbag record -o lidardata /scan

4. 2 设置环境        

        来通过rosbag记录激光雷达的数据等着以后进一步处理，bag文件会保存名字为“”lidardata”。注意 ！，输入之后不要立即执行，因为orb-slam启动需要一定时间（下面会提到），可以先输入命令，等一会再按回车执行，这样可以让orb-slam的时间戳与激光雷达的数据对的更整齐一些。然后在终端里先source 一下 orb的 setup.bash （好像并不需要，可以跳过）。

source ~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/devel/setup.bash

4. 3 启动 ORB-SLAM

        然后输入一下命令来启动 ORB-SLAM2

rosrun ORB_SLAM2 Mono ~/ORB_SLAM2/Vocabulary/ORBvoc.txt ~/ORB_SLAM2/Examples/Monocular/KITTI00-02.yaml

        这个命令里面可以看到使用了 ORBvoc.txt 和 KITTI00-02.yaml 这两个文件，这里先不详细讲述这两个文件的作用是什么，稍后会补充，反正kitti那个就是我们之前得到的相机的内参参数。运行这个命令之后，随即可以看到提示信信息 Loading ORB Vocabulary. This could take a while...，此时，我们可以在刚才录制激光雷达的终端里 按下回车（就是这个命令rosbag record -o lidardata /scan，上面提到的），开始录制激光雷达的数据，同时不久ORB-SLAM也会启动成功。如果看到弹出了两个框，一个显示的是此时摄像头的画面（Current Frame，有画面），一个显示的是点云图像（Map Viewer，一开始还啥都没有），就说明启动成功了。此时水平左右摇摆机器人，可以在Current Frame 里面 看到，突然有一瞬间出现了很多的小绿点，同时在 Map Viewer 里面产生了点云图像，这就说明机器人定位成功了，如下图，左边是刚启动时候的图像，右边是左右水平移动之后，识别了周边的环境。

        接下来，要开始真正的移动机器人来完成标定。首先要确定机器人的位置，把它放在一个环境稍微复杂的地方，并且要保证机器人能够走的开，同时还要确保机器人移动之后能回到原点形成闭环。以下是一个简单的标定环境。标定的流程可以参考一下视频。（稍后上传视频文件）

        机器人启动的时候，摄像头对着的是一块棋盘格，这里棋盘格的目的是为了增加环境的复杂度，这样在机器人启动之后，可以通过orb-slam头记录到它的初始位置，等向左转绕桌子一圈之后，回到起始位置的时，更加容易的识别到这里就是它开始的地方，从而形成闭环。如下图所示，左边那一张图就是机器人刚开始时所识别的图像，右边那一张图是点云图像，是在机器人走了一圈之后，又识别到了同样复杂的环境，从而形成了闭环。

        形成闭环的时候，会出现以下画面提示找到了闭环。

        此时便可以结束ORB-SLAM的程序 和 激光雷达的录制程序，同时可以看到，ORB的程序里面提示，Saving camera trajectory to HAHAHA123.txt 如下图

        这里的 HAHAHA123.txt 即为 ORB-SLAM 保存的 Kitti 数据集，也正是我们需要的文件。（当时这样命名完全是为了好找.....）该文件的位置位于 /home/tiger/ORB_SLAM2目录底下，同时也可以发现，这个程序还生成了一个lalala123.txt，这个文件是TUM数据集，我们这里并不需要。当然，并不一定非要走圆圈，也可以走8字等，但是环境要复杂，比如在上述的环境里，就在桌子的旁边放了很多的椅子，如果没有的话，orb-slam对着一面空空的白墙很容易就直接跟丢了目标，但是多了很多椅子腿就和背景形成了反差，更加容易识别周边的环境发生了变化。

4.4 激光雷达数据转换

        有了相机的数据集之后，我们需要来处理雷达的数据集。处理雷达的数据时，我们需要借用到一个自动记录脚本，详细可以看这里。首先，先关掉所有的终端，并在终端里面 输入 rosnode kill -a 来关闭所有ros的节点。然后找到我们刚刚rosbag 录制下来的激光雷达包，在终端里输入

rosbag play --pause lidardata.bag --clock

        其中，lidardata.bag是刚才录制好的激光雷达数据包，这个命令的作用是回放之前的激光雷达数据，但是一开始保持暂停状态，所以可以看到终端里的提示信息是 [PAUSED]，此时按下空格键将开始回放雷达数据，并使用之前录制时的ros时间。我们这时候先不要按下空格开始回放，接着执行这个命令

roslaunch cartographer_ros my.launch 

        然后再执行

roslaunch cartographer_ros visual.launch 

        这个时候将启动 Cartographer来读取激光雷达数据，可以看到已经打开了Rviz，但是还没有任何图像显示，因为之前的bag还是处于paused 状态。此时，再启动自动记录轨迹脚本，按照以下命令

source ~/hello_rospy/devel/setup.bash
rosrun beginner_tutorials csv.py 

        启动之后，会询问需要记录哪些 tf node，第一个输入 /map，第二个 输入 /laser。于是可以看到该脚本已经开始自动记录激光雷达的轨迹，并保存到 Excel 和 txt 格式的文件。

        此时，回到之前播放bag文件的终端，按下 空格 键，开始播放之前激光雷达的数据，同时可以看到 rviz里面已经开始出现图像，等到整个bag全部播放完毕后，关闭所有所有终端，之前自动记录脚本所记录的 tum.txt 即为激光雷达的数据集。