d435i和imu标定过程

1.1录制imu bag包

创建一个my_rs_camera.launch，把
arg name=“enable_gyro” default=“true”/
arg name=“enable_accel” default=“true”/
arg name=“unite_imu_method” default=“linear_interpolation”/
然后

roslaunch realsense2_camera my_rs_camera.launch

查看一下话题看有没有/camera/imu
然后录制bag包：

rosbag record -o imu_435 /camera/imu

1.2使用imu_utils进行imu内参进行标定

进入到~/catkin_ws/src/imu_utils/launch目录下：
touch my435_imu_calibration.launch
gedit my435_imu_calibration.launch
写入以下内容：

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <param name="imu_name" type="string" value= "my_imu_calibration"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "400"/>
    </node>
</launch>

1.3开始运行imu内参标定程序

roslaunch imu_utils my_d435i_imu_calibration.launch
进入到录制的bag路径中：

rosbag play -r 400 imu_435.bag

参数生成在imu_utils/data文件中。

2.1开始相机内参标定

1.有标定纸
2.标定纸的配置文件
在kalibr_workspace/src/kalibr文件下创建标定纸配置文件

touch aprilgrid.yaml
gedit aprilgrid.yaml

target_type: 'aprilgrid' #gridtype
tagCols: 6               #number of apriltags
tagRows: 6               #number of apriltags
tagSize: 0.022           #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

3.关闭结构光

roslaunch realsense2_camera my_rs_camera.launch
rosrun rqt_reconfigure rqt_reconfigure

查看是否有camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw
打开rviz
查看三个话题的图像，看一下，哪一个相机是左右
如果有的话，可以录制三个话题的bag数据包了：

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right

开始录制数据包啦！

rosbag record -o multicameras_calibration /infra_left /infra_right /color

使用相机标定的程序Kalibr进行标定：

kalibr_calibrate_cameras --target 路径/april_6x6_50x50cm_A4.yaml --bag 路径/multicameras_calibration_2020-10-29-20-19-06.bag --models pinhole-radtan pinhole-radtan pinhole-radtan --topics /infra_left /infra_right /color --bag-from-to 10 100 --show-extraction

然后在kalibr的文件中得到camchain的.yaml文件

最后啦：imu和相机标定

1.录制bag包：
首先将imu参数数据和图像对齐
复制realsense-ros包中my_rs_camera.launch，重命名为rs_imu_stereo.launch，更改内容为

arg name="enable_sync" default="true"/

2.启动launch文件

roslaunch realsense2_camera rs_imu_stereo.launch

3.关闭结构光

rosrun rqt_reconfigure rqt_reconfigure

4.打开rviz，add imu topic和infra1 topic以及infra2 topic，同时调整realsense位置，要确保双目图像数据一直包含标定板全部内容。
5.调整发布频率，和名称

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

6.录制bag包

rosbag record -o imu_stereo /infra_left /infra_right /imu

7.根据相机标定得到的文件编写相机yaml文件：camchain.yaml

cam0:
  camera_model: pinhole
  intrinsics: [461.629, 460.152, 362.680, 246.049]
  distortion_model: radtan
  distortion_coeffs: [-0.27695497, 0.06712482, 0.00087538, 0.00011556]
  T_cam_imu:
  - [0.01779318, 0.99967549,-0.01822936, 0.07008565]
  - [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
  - [0.00893160, 0.01807260, 0.99979678, 0.00399246]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.121e-05
  rostopic: /cam0/image_raw
  resolution: [752, 480]
cam1:
  camera_model: omni
  intrinsics: [0.80065662, 833.006, 830.345, 373.850, 253.749]
  distortion_model: radtan
  distortion_coeffs: [-0.33518750, 0.13211436, 0.00055967, 0.00057686]
  T_cn_cnm1:
  - [ 0.99998854, 0.00216014, 0.00427195,-0.11003785]
  - [-0.00221074, 0.99992702, 0.01187697, 0.00045792]
  - [-0.00424598,-0.01188627, 0.99992034,-0.00064487]
  - [0.0, 0.0, 0.0, 1.0]
  T_cam_imu:
  - [ 0.01567142, 0.99978002,-0.01393948,-0.03997419]
  - [-0.99966203, 0.01595569, 0.02052137,-0.01735854]
  - [ 0.02073927, 0.01361317, 0.99969223, 0.00326019]
  - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: -8.681e-05
  rostopic: /cam1/image_raw
  resolution: [752, 480]

没有的参数可以删掉

9.编写imu.yaml

#Accelerometers
accelerometer_noise_density: 1.86e-03   #Noise density (continuous-time)
accelerometer_random_walk:   4.33e-04   #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.87e-04   #Noise density (continuous-time)
gyroscope_random_walk:       2.66e-05   #Bias random walk

rostopic:                    /imu      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

10.开始标定：

kalibr_calibrate_imu_camera --bag ../imu_stereo_2020-10-30-11-24-41.bag --cam ../Aprilgrid/camchain.yaml --imu ../Aprilgrid/imu.yaml --target ../Aprilgrid/april_6x6_50x50cm_A4.yaml --show-extraction