这篇笔记我们重点记录如何使用XTDrone进行双目相机、IMU、LiDAR数据的同时采集。更具体来说，我们需要采集双目相机(左右影像)、IMU、LiDAR以及机器人位姿真值。XTDrone的基本介绍和使用在很久之前的这篇笔记中已经说过，这里不再赘述。
1. 数据采集流程
1.1 环境配置
本片笔记使用的系统环境和之前这个一样，都是Ubuntu18。不过一个小小的不同是，根据官方文档描述，由于我们这里要采集的时3D LiDAR(也就是多线LiDAR而非单线LiDAR)，所以需要给Gazebo额外装一个插件才可以正常采集数据。不然经过实际测试，即使使用了正确的LiDAR配置，但就是没有LiDAR的ROS Topic发布。配置这个额外插件也非常简单。打开一个终端，输入如下内容（具体路径参考自己环境进行修改）：
cp -r /root/Softwares/XTDrone/sitl_config/gazebo_plugin/velodyne/* /root/Softwares/xtdrone_ws/src
cd ~/catkin_ws
catkin build
上面的核心操作就是，找到了XTDrone安装目录下的sitl_config/gazebo_plugin/velodyne文件夹，然后把里面的所有内容拷贝到了一个新的ROS工作空间中（工作空间路径根据自己环境设置），然后build一下这个工作空间。最后，别忘了把这个工作空间的路径source一下放到~/.bashrc里。这样这个LiDAR插件就算安装完成了。
1.2 模型文件修改
XTDrone在PX4的安装目录/root/Softwares/PX4_Firmware/launch下自带了很多launch文件。这里我们就以indoor1.launch为例进行修改，简单贴一下：
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/indoor1.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" default="7.5"/>
<arg name="z" default="1"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_stereo_camera"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->
可以看到，这里的载荷平台(vehicle属性)用的是iris，传感器载荷是iris_stereo_camera。
首先对于运动平台，根据官方文档的描述，目前XTDrone支持多旋翼飞行器（multirotor）、固定翼飞行器（plane）、可垂直起降固定翼飞行器（vtol）以及车辆（rover）四个大类机型，multirotor下有iris、solo和typhoon_h480三个机型，vtol下有tailsitter、quadplane（模型叫standard_vtol）和tiltrotor三个机型，plane和rover没有下属机型。所以我们只需要根据需要将不同的平台名称填到vehicle属性即可。这里比如我们还是用iris无人机，不改变了。
而对于传感器载荷模块，是我们需要修改的地方。那么有哪些传感器可以用呢？答案是可以在PX4安装路径下面的Tools/sitl_gazebo/models目录去找。比如我的电脑上就是/root/Softwares/PX4_Firmware/Tools/sitl_gazebo/models。当然如果预置的都不满足要求，你完全可以自己再搭一个。另外，一个小知识，根据官方文档的描述，XTDrone中的传感器分为两类，一类是必须有的，另一类则是可以添加的。由于XTDrone的底层控制和状态估计来自于PX4，因此必须有的传感器也是来自于PX4的状态估计需求，即加速度计、陀螺仪、磁罗盘、气压计；而GPS由于需要靠mavlink通信，因此也是默认配置的。可添加的包括了单目/双目相机、深度相机与2D/3D激光雷达，typhoon_h480比较特殊，其自带了云台和相机，其余机型没有云台。可以看到，不管是什么平台，都可以认为自带了IMU，我们不再需要额外添加。
最后，由于我们要使用LiDAR，也需要简单了解一下XTDrone中支持的类型。根据官方文档描述，总体而言，对于机械LiDAR有两种：3d_lidar和3d_gpu_lidar，前者使用CPU计算，后者使用GPU计算，其他没有区别。所以，我们可以在PX4安装路径下面的Tools/sitl_gazebo/models目录找到iris_3d_gpu_lidar和iris_3d_lidar两个文件夹，如下。
所以，使用起来就非常简单了，我们只要将上面配置文件中的sdf属性的iris_stereo_camera换成iris_3d_gpu_lidar或iris_3d_lidar即可。当然了，前面也说了，iris_3d_gpu_lidar或iris_3d_lidar是XTDrone已经帮你把LiDAR和平台绑定好的可以直接使用的模块。XTDrone也提供了单独的LiDAR模块，方便你自己拼接，名字就是3d_gpu_lidar和3d_lidar(上图中的第一行第一、第二个)。所以最终换完的launch文件如下。
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/indoor1.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- iris_0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="ID_in_group" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn_xtd.launch">
<arg name="x" value="0"/>
<arg name="y" default="7.5"/>
<arg name="z" default="1"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="iris"/>
<arg name="sdf" value="iris_3d_gpu_lidar"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
<arg name="ID_in_group" value="$(arg ID_in_group)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
</launch>
<!--the launch file is generated by XTDrone multi-vehicle generator.py -->
运行仿真环境以后可以看到（后面会告诉怎么运行），我们自动载入了一个驼着LiDAR的无人机。
如果再看一下之前iris_stereo_camera模型，效果如下。
你会发现，我们虽然使用的是iris_3d_gpu_lidar，但是也有iris_stereo_camera模型里的双目相机，只是多了个LiDAR。那这就非常好了。前面说了，我们要采集双目数据，相机已经有了；IMU数据，IMU是所有机器人必备的，也有了；LiDAR数据，我们通过iris_3d_gpu_lidar已经添加了。也就是说，iris_3d_gpu_lidar这个模型已经完全可以满足我们的需求了，省了很多时间。
1.3 开始采集
下面就介绍数据采集的核心步骤，内容其实和之前的这篇笔记是一样的。但为了完整性，这里再简单贴一下步骤。如果需要看详细解释，移步上面的笔记。
1.3.1 运行仿真环境
打开一个终端，输入如下命令，启动Gazebo仿真环境运行刚刚修改好的launch文件：
roslaunch px4 indoor1-lidar.launch
效果如下：
1.3.2 开启通讯节点
打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令(注意，需要把第一个命令中的XTDrone路径换成你自己的)，启动通讯节点：
cd ~/Softwares/XTDrone/communication
python multirotor_communication.py iris 0
正常执行完上面的命令后，你应该就会看到“iris_0: communication initialized”的提示，如下所示。
1.3.3 启动位姿转发节点
再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令(注意，需要把第一个命令中的XTDrone路径换成你自己的)，启动位姿转发节点。
cd ~/Softwares/XTDrone/sensing/pose_ground_truth
python get_local_pose.py iris 1
默认情况下XTDrone并不会自动发布位姿真值，需要我们输入上面的命令手动发布。这里的1表示1架无人机。默认发布的Topic名称为/iris_0/mavros/vision_pose/pose。执行完上面的命令后，你应该就会看到“Get iris_0 groundtruth pose”的提示，如下所示。
1.3.4 启动可视化影像节点
再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令，用于实时可视化相机数据：
rqt_image_view /iris_0/stereo_camera/left/image_raw
可视化影像的目的是，方便我们更好控制采集数据的质量，方便随时调整。这里，iris无人机本身自带了一个双目相机，分别发布左右影像的Topic，为/iris_0/stereo_camera/left/image_raw和/iris_0/stereo_camera/right/image_raw。我们订阅左影像的数据就够了，效果如下。
1.3.5 开启控制脚本
再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令(注意，需要把第一个命令中的XTDrone路径换成你自己的)，用于实时控制无人机：
cd ~/Softwares/XTDrone/control/keyboard
python multirotor_keyboard_control.py iris 1 vel
运行以后出现如下界面就表示成功了。
XTDrone推荐的推荐起飞流程: 按i把向上速度加到0.3以上，再按b切offboard模式，最后按t解锁。基本的关于控制无人机的按键包括：w-前进、x-后退、i-上升、,-下降、j-左转、l-右转。 更多信息，可以参考官方文档。
1.3.6 录制ROS Bag数据包
再打开一个新终端(快捷键Ctrl+Alt+T)，输入如下命令，用于录制想要的Topic:
rosbag record /iris_0/stereo_camera/left/image_raw /iris_0/stereo_camera/right/image_raw /iris_0/imu_gazebo /iris_0/mavros/vision_pose/pose /iris_0/velodyne_points
这里简单解释一下：双目相机的Topic名称没什么好说的，前面说过了；IMU的Topic是/iris_0/imu_gazebo，而如果你细心会发现还有一个/iris_0/mavros/imu/data，它们都是ROS的IMU数据类型。到底该用谁呢？其实根据名称就可以看出来，后者是由MAVROS发布的，我们更倾向使用前者。事实上，在官方文档的VINS示例中，使用的也是这个Topic；LiDAR的Topic默认就是/iris_0/velodyne_points，也没什么特殊的；而轨迹真值的Topic名称，在1.3.3部分已经说过了，默认就叫这个。
最后，我们用上面的命令简单录制了一个简短的Bag包，查看信息如下。
可以看到，IMU帧率大约为200Hz，轨迹真值约为30Hz，双目影像的帧率约为19FPS，LiDAR的帧率约为15FPS。我们可以利用rqt_bag工具进一步查看数据帧率的分布情况，如下。
总体而言还算是比较均匀。我们还可以用RViz播放一下刚刚我们采集的LiDAR点云数据，如下。
至此，数据采集流程全部完成。
2. 传感器内参的读取与设置
最后，我们来研究我们用XTDrone采集数据传感器的内参，其实核心就是sdf配置文件。前面也说了，我们这里使用的是iris_3d_gpu_lidar传感器模块，所以双目、IMU、LiDAR的相关参数都能在这里找到。我们首先把iris_3d_gpu_lidar.sdf中和传感器有关的内容贴出来(完整文件好几百行，很多不相关内容)。
<!-- For Velodyne Lidar Payload -->
<include>
<uri>model://3d_gpu_lidar</uri>
<pose>0 0 0.08 0 0 0</pose>
</include>
<joint name="lidar_joint" type="fixed">
<child>3d_gpu_lidar::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- For Stereo Camera Payload -->
<include>
<uri>model://stereo_camera</uri>
<pose>0.1 0 0 0 0 0</pose>
</include>
<joint name="stereo_joint" type="fixed">
<child>stereo_camera::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- For IMU-->
<include>
<uri>model://imu_gazebo</uri>
<pose>0 0 -0.05 0 0 0</pose>
</include>
<joint name="imu_gazebo_joint" type="fixed">
<child>imu_gazebo::link</child>
<parent>base_link</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
可以看到，我们需要的双目相机、LiDAR、IMU都有了，而其实每一个传感器又都是include其它的更小的模块：stereo_camera、3d_gpu_lidar、imu_gazebo。我们只需要在models文件夹中去找这些文件夹即可。但有一点千万要注意，一个文件夹里可能会有多个sdf文件，所以我们一定要找到根sdf文件(Gazebo使用的那个)，否则你修改了半天也没效果。如何找到使用的到底是哪个sdf文件呢？答案在model.config里。这个文件是每个Gazebo模型必须要有的配置文件。打开以后，会看到里面有一个sdf属性。这个属性指向的sdf文件才是这个模型真正使用的文件。比如，stereo_camera这个模块，他下面就有model.sdf和stereo_camera.sdf两个sdf文件。
你不能简单根据stereo_camera.sdf名称和模型名相同而就认为使用的就是他。我们只有打开model.config才能看到“庐山真面目”，如下。
可以看到，这里用的是model.sdf。所以这一点在实际使用中需要注意。下面我们会进一步介绍每个模块。
2.1 双目相机
在sdf文件中，通过include标签，指定了uri，明确双目相机的模型是stereo_camera。所以我们就去找models文件夹中stereo_camera文件夹，找到model.config。看看他用的是哪个sdf文件。发现用的是model.sdf，打开如下。
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="stereo_camera">
<link name="link">
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.00000166667</ixx>
<iyy>0.00000166667</iyy>
<izz>0.00000166667</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<box>
<size>0.015 0.1 0.02</size>
</box>
</geometry>
</visual>
<sensor name="camera" type="multicamera">
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>false</visualize>
<camera name="left">
<pose>0 0.06 0 0 0 0</pose>
<horizontal_fov>1.5708</horizontal_fov>
<image>
<width>752</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>300</far>
</clip>
</camera>
<camera name="right">
<pose>0 -0.06 0 0 0 0</pose>
<horizontal_fov>1.5708</horizontal_fov>
<image>
<width>752</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>300</far>
</clip>
</camera>
<plugin name="stereo_camera_controller" filename="libgazebo_ros_multicamera.so">
<robotNamespace></robotNamespace>
<alwaysOn>true</alwaysOn>
<updateRate>30</updateRate>
<cameraName>/stereo_camera</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>stereo_camera_frame</frameName>
<hackBaseline>0.12</hackBaseline>
<Fx>376.0</Fx>
<Fy>376.0</Fy>
<Cx>376.0</Cx>
<Cy>240.0</Cy>
<distortionK1>-0.1</distortionK1>
<distortionK2>0.01</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>5e-5</distortionT1>
<distortionT2>-1e-4</distortionT2>
</plugin>
</sensor>
</link>
</model>
</sdf>
可以看到，这里有非常多我们感兴趣的参数。由于我们这一部分先关心内参，所以几何相关的外参我们暂时不看。整个文件的结构根节点是sdf，然后只包含了一个model节点。在model节点下，又只包含了一个link节点。link节点又进一步包含了inertial、visual、sensor三个节点，分别负责模型惯性相关的属性、视觉可视化相关的属性、传感器本身的属性。
这里我们重点关注的就是sensor节点。在他下面，包含两个camera节点，分别代表了左右两个相机相关的参数，而plugin节点则是包含了一些和ROS Topic名称、相机畸变参数、内参等一系列重要内容。下面就重点相关的参数进行介绍：
camera.horizontal_fov: 相机的水平视场角，弧度表示camera.image.width: 影像宽度，以像素表示camera.image.height: 影像高度，以像素表示sensor.plugin.Fx: 相机的x方向上的焦距sensor.plugin.Fy: 相机的y方向上的焦距sensor.plugin.Cx: 相机的x方向上的主点sensor.plugin.Cy: 相机的y方向上的主点sensor.plugin.distortionK1: 相机的畸变参数K1，对应径向畸变sensor.plugin.distortionK2: 相机的畸变参数K2，对应径向畸变sensor.plugin.distortionK3: 相机的畸变参数K3，对应径向畸变sensor.plugin.distortionT1: 相机的畸变参数T1，对应切向畸变-
sensor.plugin.distortionT2: 相机的畸变参数T2，对应切向畸变 camera.pose: 相机传感器在模块中的相对位姿，由6个数字组成，分别是: x y z roll pitch yaw，单位为米和弧度camera.clip.near: 影像观测的最近距离，单位为米camera.clip.far: 影像观测的最远距离，单位为米-
sensor.plugin.hackBaseline: 双目相机的基线长度，单位米 camera.image.format: 影像数据的ROS消息格式sensor.plugin.cameraName: 相机名称sensor.plugin.imageTopicName: 相机数据Topic的名称sensor.plugin.cameraInfoTopicName: 相机信息的Topic名称sensor.plugin.frameName: 相机数据的参考坐标系名称sensor.update_rate: 相机Topic发布的帧率（在sensor.plugin下面也有一个update_date属性，但经过实测修改它不会有影响）
至此，我们就介绍完了双目相机相关的参数以及设置方法。
2.2 IMU
和双目相机类似的，这里我们使用的IMU模块为imu_gazebo，打开对应的sdf文件如下。
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="imu_gazebo">
<link name="link">
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.000000166667</ixx>
<iyy>0.000000166667</iyy>
<izz>0.000000166667</izz>
</inertia>
</inertial>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<parent>link</parent>
<update_rate>250</update_rate>
<visualize>true</visualize>
<topic>imu_gazebo</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<topicName>imu_gazebo</topicName>
<bodyName>imu_link_stereo</bodyName>
<updateRateHZ>250.0</updateRateHZ>
<gaussianNoise>0.0</gaussianNoise>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
<frameName>imu_link_stereo</frameName>
<gyroscopeNoiseDensity>0.0006</gyroscopeNoiseDensity>
<gyroscopeRandomWalk>0.000003</gyroscopeRandomWalk>
<gyroscopeTurnOnBiasSigma>0.03</gyroscopeTurnOnBiasSigma>
<accelerometerNoiseDensity>0.002</accelerometerNoiseDensity>
<accelerometerRandomWalk>0.00002</accelerometerRandomWalk>
<accelerometerTurnOnBiasSigma>0.1</accelerometerTurnOnBiasSigma>
</plugin>
<pose>0 0 0.3 0 0 0</pose>
</sensor>
</link>
</model>
</sdf>
整个文件的结构和双目是类似的。这里直接贴出来相关重要属性：
sensor.update_rate: IMU帧率设置sensor.plugin.topicName: IMU数据Topic名称sensor.plugin.bodyName: IMU固连接的本体名称sensor.plugin.xyzOffset: IMU在的相对位置，单位为米sensor.plugin.rpyOffset: IMU在的相对姿态，单位为弧度sensor.plugin.frameName: IMU坐标系的名称sensor.plugin.gyroscopeNoiseDensity: 陀螺仪噪声密度sensor.plugin.gyroscopeRandomWalk: 陀螺仪随机游走sensor.plugin.gyroscopeTurnOnBiasSigma: 陀螺仪偏置稳定性sensor.plugin.accelerometerNoiseDensity: 加速度计噪声密度sensor.plugin.accelerometerRandomWalk: 加速度计随机游走sensor.plugin.accelerometerTurnOnBiasSigma: 加速度计偏置稳定性sensor.pose: IMU相对于安装位置的相对位姿，由6个数字组成，分别是: x y z roll pitch yaw，单位为米和弧度
至此，IMU的核心参数就介绍完了。
2.3 LiDAR
和双目相机类似的，这里我们使用的LiDAR模块为3d_gpu_lidar，打开对应的sdf文件如下。
<?xml version="1.0"?>
<sdf version="1.5">
<model name="3d_lidar">
<link name="link">
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.001</mass>
<inertia>
<ixx>4.15e-6</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2.407e-6</iyy>
<iyz>0</iyz>
<izz>2.407e-6</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<mesh>
<uri>model://hokuyo/meshes/hokuyo.dae</uri>
</mesh>
</geometry>
</visual>
<sensor name='3d_lidar' type='gpu_ray'>
<ray>
<scan>
<horizontal>
<samples>512</samples>
<resolution>1</resolution>
<min_angle>-3.1415926535897931</min_angle>
<max_angle>3.1415926535897931</max_angle>
</horizontal>
<vertical>
<samples>32</samples>
<min_angle>-0.2617993877991494365</min_angle>
<max_angle>0.2617993877991494365</max_angle>
</vertical>
</scan>
<range>
<min>0.2</min>
<max>30</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin name='3d_laser' filename='libgazebo_ros_velodyne_gpu_laser.so'>
<robotNamespace></robotNamespace>
<topicName>/velodyne_points</topicName>
<frameName>laser_3d</frameName>
<gaussianNoise>0.008</gaussianNoise>
</plugin>
<always_on>1</always_on>
<update_rate>15</update_rate>
<visualize>0</visualize>
</sensor>
</link>
</model>
</sdf>
整个文件的结构和双目是类似的。这里直接贴出来sensor中的相关重要属性：
sensor.ray.scan.horizontal.samples: 水平方向上的采样数量sensor.ray.scan.horizontal.min_angle: 水平方向起始角度，-pi到pi之间，弧度表示sensor.ray.scan.horizontal.max_angle: 水平方向中止角度，-pi到pi之间，弧度表示sensor.ray.scan.vertical.samples: 竖直方向上的采样数量sensor.ray.scan.vertical.min_angle: 竖直方向起始角度，-pi/2到pi/2之间，弧度表示sensor.ray.scan.vertical.max_angle: 竖直方向中止角度，-pi/2到pi/2之间，弧度表示sensor.ray.range.min: 最小量程距离，单位米sensor.ray.range.max: 最大量程距离，单位米-
sensor.ray.range.resolution: 距离分辨率，单位米 sensor.plugin.topicName: LiDAR Topic名称sensor.plugin.frameName: LiDAR坐标系的名称-
sensor.plugin.gaussianNoise: LiDAR测量的高斯噪声 sensor.update_rate: LiDAR测量的帧率
至此，LiDAR的核心参数就介绍完了。
3.传感器外参计算
计算传感器之间的外参核心就是从配置文件中找到各个传感器的位置和坐标系定义，然后求解彼此之间的变换关系。我们在iris_3d_gpu_lidar文件中可以很清晰地找到LiDAR、IMU、双目相机的位置，如下。
可以看到每个传感器都设置了自己的pose属性，由x、y、z、roll、pitch、yaw组成。同时可以看出，每个传感器都通过一个类型为fixed的joint与base_link相连接。而关于Gazebo中的joint，详情可以参考这个官方文档。如果是fixed的joint，那么下面的axis属性其实是不起作用的。所以根据这个配置文件，我们可以得到使用的3d_gpu_lidar, stereo_camera, imu_gazebo的位姿，如下。
这些位姿我们认为是在base_link下表达的(可以把base_link当做是一种本体坐标系)。进一步在每一个模型里面，又有相对关系。我们可以分别打开各自配置文件进行查看。而对于LiDAR、双目相机、IMU坐标系的指向问题，Gazebo以及配置文件中并没有明确提及，所以我们就暂且假设：本体坐标系与世界坐标系相同，世界坐标系的指向为X轴指向前方，Y轴指向左方，Z轴指向上方。
对于IMU坐标系，以无人机或小车为原点，与本体坐标系同指向(X轴指向前方，Y轴指向左方，Z轴指向上方)，单位为米。对于LiDAR坐标系，以LiDAR为原点，与本体坐标系同指向(X轴指向前方，Y轴指向左方，Z轴指向上方)，单位为米。对于相机坐标系，以相机为原点，X轴指向相机右方，Y轴指向相机下方，Z轴指向相机前方，单位为米。
对于3d_gpu_lidar，打开对应配置文件，可以发现，其中并没有额外的变换关系了。也就是说，LiDAR传感器在base_link(本体)坐标系下的坐标就是这个。
对于stereo_camera，打开配置文件，如下。
可以分别找到，左右相机相对于双目相机模块的坐标分别为:
这个坐标我们认为是在前面指定的双目相机模块位置、指向和base_link(本体系)相同的坐标系，注意和相机坐标系是不同的。
对于imu_gazebo，打开配置文件，如下。
可以看到，IMU的相对位姿为：
和双目相机类似的，我们认为这个位姿是在前面指定的IMU模块位置、指向和base_link(本体系)相同的坐标系。
前面说了，LiDAR、IMU、双目相机三个模型的位姿是表示在base_link下的，而每个模型内部的位姿又是相对于在指定原点，指向和base_link相同的坐标系下的。所以，我们完全可以把所有的位置统一到base_link下，如下。
如同之前笔记说的，外参求解本质上就是解算各个传感器坐标系之间的变换关系，简单的示意图如下。
所以求解过程和之前笔记是一样的。
本体到IMU的变换\(T_{bi}\)。根据参数文件，IMU在base_link坐标系下的坐标为(0,0,0.25)，指向完全相同。所以\(T_{bi}\)为：
\[T_{bi} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0.25\\ 0 & 0 & 0 & 1 \end{bmatrix}\]本体到LiDAR的变换\(T_{bl}\)。 根据参数文件，LiDAR在base_link坐标系下的坐标为(0,0,0.08)，指向完全相同。所以\(Tbl\)为：
\[T_{bl} = \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0.08\\ 0 & 0 & 0 & 1 \end{bmatrix}\]本体到左相机的变换\(T_{bcl}\)。左相机在base_link坐标系下的坐标为(0.1, 0.06, 0)，但坐标轴指向不同。一个简单的旋转方法是，先绕着本体坐标系的Y轴转90度，再绕着本体坐标系的Z轴转90度就和本体坐标系指向相同了。我们定义旋转方向：面对旋转轴正向，顺时针为正，逆时针为负。因此，绕Y轴旋转为正向90度，绕Z轴旋转为负向90度。因此，\(T_{bcl}\)为：
\[T_{bcl} = \begin{bmatrix} R_{Y}(90)R_{Z}(-90) & t\\ 0 & 1 \end{bmatrix}=\begin{bmatrix} 0 & 0 & 1 & 0.1\\ -1 & 0 & 0 & 0.06\\ 0 & -1 & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\]本体到右相机的变换\(T_{bcr}\)。右相机也是类似的，只是平移部分不同，如下。
\[T_{bcr} = \begin{bmatrix} R_{Y}(90)R_{Z}(-90) & t\\ 0 & 1 \end{bmatrix}=\begin{bmatrix} 0 & 0 & 1 & 0.1\\ -1 & 0 & 0 & -0.06\\ 0 & -1 & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\]得到基本变换以后，再利用他们合成传感器之间的变换关系。一般而言，我们会选择相机(如果是双目那就是左相机)作为参照，让其它传感器变换到它坐标系下。这里需要计算三个变换：IMU到左相机的变换、LiDAR到左相机的变换、右相机到左相机的变换。根据上面的坐标系关系图，计算公式分别如下。
\[\left\{\begin{matrix} E_{IMU-CAM_{L}} = T_{bi}^{-1}T_{bcl} \\ E_{LiDAR-CAM_{L}} = T_{bl}^{-1}T_{bcl}\\ E_{CAM_{R}-CAM_{L}} = T_{bcr}^{-1}T_{bcl} \end{matrix}\right.\]把上面计算出来的具体变换带入即可求解得到彼此之间的变换关系。
4.参考资料
- [1] https://www.yuque.com/xtdrone/manual_cn/3d_laserslam
- [2] https://www.yuque.com/xtdrone/manual_cn/vehicle_config
- [3] https://www.yuque.com/xtdrone/manual_cn/sensor_config
- [4] https://www.yuque.com/xtdrone/manual_cn/3d_laserslam#mBtiv
- [5] https://www.yuque.com/xtdrone/manual_cn/basic_config#fN3kj
- [6] https://www.yuque.com/xtdrone/manual_cn/vio#luwyy
- [7] http://wiki.ros.org/urdf/XML/joint
本文作者原创,未经许可不得转载,谢谢配合
