# vins-fusion问题总结

## T265

* 官方标定参数获取

  可使用命令 `rs-enumerate-devices -c` 获取

* vins-fuion中T265内、外参

  参考文章[vins-fusion中T265内外参的调整](https://blog.csdn.net/weixin_45810108/article/details/116917579)，由于T265是语言模型，所以YAML文件中的 `model_type` 应设置为 `KANNALA_BRANDT` ，其余参数详见下表：

  left.yaml：

  ```YAML
  %YAML:1.0
  ---
  model_type: KANNALA_BRANDT
  camera_name: camera
  image_width: 848
  image_height: 800
  
  projection_parameters:
     k2: -0.00883461162447929
     k3: 0.0455518402159214
     k4: -0.0433646589517593
     k5: 0.00801306683570147
     mu: 285.555908203125
     mv: 285.809295654297
     u0: 416.527496337891
     v0: 382.654296875
  ```

  right.yaml：

  ```yaml
  %YAML:1.0
  ---
  model_type: KANNALA_BRANDT
  camera_name: camera
  image_width: 848
  image_height: 800
  
  projection_parameters:
     k2: -0.00875154510140419
     k3: 0.0462869592010975
     k4: -0.0439813211560249
     k5: 0.008194868452847
     mu: 286.065399169922
     mv: 286.259704589844
     u0: 411.838592529297
     v0: 382.638397216797
  ```

  T265_imu_config.yaml：

  ```yaml
  %YAML:1.0
  
  #common parameters
  #support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
  imu: 1         
  num_of_cam: 2  
  
  imu_topic: "/camera/imu"
  image0_topic: "/camera/fisheye1/image_raw"
  image1_topic: "/camera/fisheye2/image_raw"
  output_path: "/root/catkin_ws/src/output"
  
  cam0_calib: "left.yaml"
  cam1_calib: "right.yaml"
  image_width: 848
  image_height: 800
     
  
  # Extrinsic parameter between IMU and Camera.
  estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                          # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
  
  body_T_cam0: !!opencv-matrix
     rows: 4
     cols: 4
     dt: d
     data: [-0.999971, 0.00449623, 0.00612845, 0.0107000004500151,
            -0.00447373, -0.999983, 0.0036805, -3.63797880709171e-12,
             0.0061449, 0.00365298, 0.999974, 1.45519152283669e-11,
             0, 0, 0, 1]
  
  body_T_cam1: !!opencv-matrix
     rows: 4
     cols: 4
     dt: d
     data: [-0.999933, 0.0110816, 0.00339323, -0.0535899698734283,
            -0.0110778, -0.999938, 0.0011195, -0.000334849231876433,
            0.00340542, 0.00108183, 0.999994, 0.000142708333441988,
            0, 0, 0, 1]
  
  #Multiple thread support
  multiple_thread: 1
  
  #feature traker paprameters
  max_cnt: 150            # max feature number in feature tracking
  min_dist: 30            # min distance between two features 
  freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
  F_threshold: 1.0        # ransac threshold (pixel)
  show_track: 1           # publish tracking image as topic
  flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
  
  #optimization parameters
  max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
  max_num_iterations: 8   # max solver itrations, to guarantee real time
  keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
  
  #imu parameters       The more accurate parameters you provide, the better performance
  acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
  gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
  acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
  gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
  g_norm: 9.805         # gravity magnitude
  
  #unsynchronization parameters
  estimate_td: 1                      # online estimate time offset between camera and imu
  td: 0.00                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
  
  #loop closure parameters
  load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
  pose_graph_save_path: "/root/catkin_ws/src/output/vins-fuison/" # save and load path
  save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
  ```

  之后对于不同的T265相机可参考以上内容进行参数的修改。

* 待解决的问题
  * 用官方参数后定位数据偏差仍较大。

## D435i

* 官方参数获取同上

* vins-fusion中参数

  需要注意的是，D435i为针孔模型，所以 `model_type` 为 `PINHOLE` 。

  left.yaml

  ```yaml
  %YAML:1.0
  ---
  model_type: PINHOLE
  camera_name: camera
  image_width: 640
  image_height: 480
  distortion_parameters:
     k1: 0.0
     k2: 0.0
     p1: 0.0
     p2: 0.0
  projection_parameters:
     fx: 383.692
     fy: 383.692
     cx: 316.978
     cy: 239.796
  ```

  right.yaml

  ```yaml
  %YAML:1.0
  ---
  model_type: PINHOLE
  camera_name: camera
  image_width: 640
  image_height: 480
  distortion_parameters:
     k1: 0.0
     k2: 0.0
     p1: 0.0
     p2: 0.0
  projection_parameters:
     fx: 383.692
     fy: 383.692
     cx: 316.978
     cy: 239.796
  ```

  realsense_stereo_imu_config.yaml

  ```yaml
  %YAML:1.0
  
  #common parameters
  #support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
  imu: 1         
  num_of_cam: 2  
  
  imu_topic: "/camera/imu"
  image0_topic: "/camera/infra1/image_rect_raw"
  image1_topic: "/camera/infra2/image_rect_raw"
  output_path: "/root/catkin_ws/src/output"
  
  cam0_calib: "left.yaml"
  cam1_calib: "right.yaml"
  image_width: 640
  image_height: 480
     
  
  # Extrinsic parameter between IMU and Camera.
  estimate_extrinsic: 1   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                          # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
  
  body_T_cam0: !!opencv-matrix
     rows: 4
     cols: 4
     dt: d
     data: [ 1, 0, 0, -0.00552,
             0, 1, 0, 0.0051,
             0, 0, 1, 0.01174,
             0, 0, 0, 1 ]
  
  body_T_cam1: !!opencv-matrix
     rows: 4
     cols: 4
     dt: d
     data: [ 1, 0, 0, 0.0446571,
             0, 1, 0, 0.0051,
             0, 0, 1, 0.01174,
             0, 0, 0, 1 ]
  
  #Multiple thread support
  multiple_thread: 1
  
  #feature traker paprameters
  max_cnt: 150            # max feature number in feature tracking
  min_dist: 30            # min distance between two features 
  freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
  F_threshold: 1.0        # ransac threshold (pixel)
  show_track: 1           # publish tracking image as topic
  flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy
  
  #optimization parameters
  max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
  max_num_iterations: 8   # max solver itrations, to guarantee real time
  keyframe_parallax: 10.0 # keyframe selection threshold (pixel)
  
  #imu parameters       The more accurate parameters you provide, the better performance
  acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
  gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
  acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
  gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
  g_norm: 9.805         # gravity magnitude
  
  #unsynchronization parameters
  estimate_td: 1                      # online estimate time offset between camera and imu
  td: 0.00                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
  
  #loop closure parameters
  load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
  pose_graph_save_path: "/root/catkin_ws/src/output/vins-fuison/" # save and load path
  save_image: 0                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
  ```

  