T265+D430跟踪建图

番外更新：T265和D430的联合定位建图

一、T265以及D430的launch文件，内部坐标关系及其 tf 树

我采用T265用于输出里程计信息，D430用于输出深度信息，此外为了做无人机控制还需要利用px4飞控的imu信息。三个传感器之间复杂的坐标关系很容易把人绕晕。
首先需要配置realsense2_camera 的launch文件，来同时打开T265相机和D430相机。launch文件的配置可以参考这篇文章。

<launch>
  <arg name="device_type_camera1"    			default="t265"/>
  <arg name="device_type_camera2"    			default="d430"/>		
  <arg name="serial_no_camera1"    			default="230222110093"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="serial_no_camera2"    			default="140122076800"/> 			<!-- Note: Replace with actual serial number -->
  <arg name="camera1"              			default="t265"/>		<!-- Note: Replace with camera name -->
  <arg name="camera2"              			default="d430"/>		<!-- Note: Replace with camera name -->
  <arg name="tf_prefix_camera1"         	default="$(arg camera1)"/>
  <arg name="tf_prefix_camera2"         	default="$(arg camera2)"/>
  <arg name="initial_reset"             	default="false"/>
  <arg name="enable_sync"               	default="true"/>

  <!-- t265 -->
  <arg name="fisheye_width"       		default="640"/> 
  <arg name="fisheye_height"      		default="480"/>
  <arg name="fisheye_fps"         		default="-1"/>
  <arg name="gyro_fps"            		default="200"/>
  <arg name="accel_fps"           		default="200"/>
  <arg name="enable_gyro"         		default="true"/>
  <arg name="enable_accel"        		default="true"/>
  <arg name="enable_fisheye1"     		default="false"/>
  <arg name="enable_fisheye2"     		default="false"/>
  <arg name="linear_accel_cov"    		default="0.01"/>
  <arg name="unite_imu_method"    		default=""/>
  <arg name="topic_odom_in"       		default="odom_in"/>
  <arg name="calib_odom_file"     		default=""/>
  <!-- end t265 -->

  <!-- d430 -->
  <arg name="depth_width"           		default="640"/>
  <arg name="depth_height"          		default="480"/>
  <arg name="depth_fps"            		default="30"/>
  <arg name="infra_width"           		default="640"/>
  <arg name="infra_height"         		default="480"/>
  <arg name="infra_fps"                 	default="30"/>
  <arg name="enable_depth"              	value="true"/>
  <arg name="enable_color"              	value="false"/>
  <arg name="enable_infra"             	value="true"/>
  <arg name="enable_infra1"            	value="true"/>
  <arg name="enable_infra2"             	value="true"/>
  <arg name="enable_pointcloud"         	value="true"/>
  <arg name="filters"                   	value="pointcloud"/>

  <!-- end d430 -->

  <group ns="$(arg camera1)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="serial_no"             	value="$(arg serial_no_camera1)"/>
      <arg name="tf_prefix"         		value="$(arg tf_prefix_camera1)"/>
      

      <arg name="fisheye_width"            	value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           	value="$(arg fisheye_height)"/>
      
      <arg name="enable_fisheye1"          	value="$(arg enable_fisheye1)"/>
      <arg name="enable_fisheye2"          	value="$(arg enable_fisheye2)"/>
      <arg name="fisheye_fps"              	value="$(arg fisheye_fps)"/>
      
      <arg name="gyro_fps"                 	value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                	value="$(arg accel_fps)"/>
      
      <arg name="enable_gyro"              	value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             	value="$(arg enable_accel)"/>
      
      <arg name="enable_color"             	value="false"/>
      <arg name="enable_depth"             	value="false"/>
      <arg name="enable_pose"             	value="true"/>
      
      <arg name="linear_accel_cov"         	value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            	value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         	value="$(arg unite_imu_method)"/>

      <arg name="topic_odom_in"         	value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"       	value="$(arg calib_odom_file)"/>

      <arg name="enable_sync"              	value="$(arg enable_sync)"/>
    </include>
  </group>

  <group ns="$(arg camera2)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="serial_no"             	value="$(arg serial_no_camera2)"/>
      <arg name="tf_prefix"			value="$(arg tf_prefix_camera2)"/>
      <arg name="initial_reset"         	value="$(arg initial_reset)"/>

      <arg name="depth_width"       		value="$(arg depth_width)"/>
      <arg name="depth_height"      		value="$(arg depth_height)"/>
      <arg name="depth_fps"         		value="$(arg depth_fps)"/>
      
      <arg name="infra_width"       		value="$(arg infra_width)"/>
      <arg name="infra_height"      		value="$(arg infra_height)"/>
      <arg name="infra_fps"         		value="$(arg infra_fps)"/>
      
      <arg name="enable_depth"      		value="$(arg enable_depth)"/>
      <arg name="enable_color"      		value="$(arg enable_color)"/>
      <arg name="enable_infra"     		value="$(arg enable_infra)"/>
      <arg name="enable_infra1"     		value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"     		value="$(arg enable_infra2)"/>
      <arg name="enable_pointcloud" 		value="$(arg enable_pointcloud)"/>
      <arg name="filters"           		value="$(arg filters)"/>
      <arg name="enable_sync"       		value="$(arg enable_sync)"/>

    </include>
  </group>

</launch>

打开命令行，运行rs-enumerate-devices -c，可以查看rs相机出场时标定的内外参。

T265出厂参数给出的各坐标系关系如图，其中值得注意的是Pose坐标系，经过rs2_camera驱动会自动输出为ROS下的北西天坐标系。其位置关系可以通过出厂参数获取。
然后，命令行运行rosrun rqt_tf_tree rqt_tf_tree可以查看各相机的tf坐标树。
运行rosrun tf tf_echo tf_1 tf_2可以查看两个tf之间的关系。

对于D430：
D430_link <-固连-> D430_depth_frame <-固连-> D430_infra1_frame
对于T265：
t265_odom_frame <-不固连-> t265_pose_frame <-固连-> t265_link
对于px4：
base_link

我们可以通过单目＋imu联合标定的方法，获取D430_link、t265_link、base_link之间的坐标关系，即可完成三个传感器的数据融合。

二、单目相机+imu联合标定

通过上面的步骤可以发现，D430的基础系、深度系都是固连在左目系上的，因此可以考虑使用kalibrate来获取D430和px4imu的外参。并且T265的imu到Pose的位置关系我们也是知道的，那么也可以用联合标定的方式搞定D430和T265的外参。
相机和imu联合标定可以看这篇帖子：jetson xavier nx 安装系统+ros+opencv-gpu+VINS-GPU+vins标定。
详细的标定过程可能后面会开一个新帖专门来记录一下。

三、T265报错：RS2_USB_STATUS_NO_DEVICE

运行：

sudo cp ~/.99-realsense-libusb.rules /etc/udev/rules.d/99-realsense-libusb.rules 
sudo udevadm control --reload-rules && udevadm trigger

成功解决。