Running s-msckf on ZED2

[truncs]

Hi,

I am trying to run s-msckf on ZED2 cameras but I am keep running into issues with run off velocity in some direction. Would love to get some pointers on how to debug this issue. Attaching a GIF for reference along with my calibration files from Kalibr, launch files for ZED2.

camchain-imucam-zed.yaml

cam0:
  T_cam_imu:
  - [0.013911805424657586, -0.9999014854151124, 0.0018657798587341512, 0.022094309694273395]
  - [-0.001896877699639643, -0.00189234857879228, -0.9999964104294835, 0.008222404533393322]
  - [0.9999014269040835, 0.013908216331044554, -0.0019230168147442717, -0.004019943949167127]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [-0.03145355965080863, 0.03963371222227746, -0.00012991142188599494,
    -0.0009841405231984133]
  distortion_model: radtan
  intrinsics: [529.6064288823629, 531.6487692519067, 662.4595237181235, 359.46596548891574]
  resolution: [1280, 720]
  rostopic: /zed2/zed_node/left/image_rect_color
  timeshift_cam_imu: -0.011451655869953541
cam1:
  T_cam_imu:
  - [0.014595083879945875, -0.999891600462897, 0.0019418677307252996, -0.0974173564292532]
  - [-0.0012541816469447409, -0.001960379862688766, -0.9999972919659283, 0.008240758435516514]
  - [0.9998926995207704, 0.014592608901092763, -0.0012826576027671899, -0.0013916911014737843]
  - [0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
  - [0.999999763621736, -7.740240030827405e-05, 0.0006832022662123308, -0.11950827803224877]
  - [7.696399950239134e-05, 0.9999997911563736, 0.0006416882015509623, 1.9234703482717486e-05]
  - [-0.0006832517917369429, -0.0006416354678909438, 0.9999995607353593, 0.0026486228449398917]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [-0.043870206407236524, 0.06254148649507542, 5.061236789252414e-05,
    -0.0010133214263416475]
  distortion_model: radtan
  intrinsics: [531.6826351410144, 533.7563616638154, 661.9758334694967, 358.8698219756683]
  resolution: [1280, 720]
  rostopic: /zed2/zed_node/right/image_rect_color
  timeshift_cam_imu: -0.011376154980228488
T_imu_body:
  [1.0000, 0.0000, 0.0000, 0.0000,
  0.0000, 1.0000, 0.0000, 0.0000,
  0.0000, 0.0000, 1.0000, 0.0000,
  0.0000, 0.0000, 0.0000, 1.0000]

msckf_vio_zed.launch

<launch>

  <arg name="robot" default="zed"/>
  <arg name="fixed_frame_id" default="base_link"/>
  <arg name="calibration_file"
    default="$(find msckf_vio)/config/camchain-imucam-zed.yaml"/>

  <!-- Image Processor Nodelet  -->
  <include file="$(find msckf_vio)/launch/image_processor_zed.launch">
    <arg name="robot" value="$(arg robot)"/>
    <arg name="calibration_file" value="$(arg calibration_file)"/>
  </include>

  <!-- Msckf Vio Nodelet  -->
  <group ns="$(arg robot)">
    <node pkg="nodelet" type="nodelet" name="vio"
      args='standalone msckf_vio/MsckfVioNodelet'
      output="screen">

      <!-- Calibration parameters -->
      <rosparam command="load" file="$(arg calibration_file)"/>

      <param name="publish_tf" value="true"/>
      <param name="frame_rate" value="15"/>
      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
      <param name="child_frame_id" value="odom"/>
      <param name="max_cam_state_size" value="15"/>
      <param name="position_std_threshold" value="8.0"/>

      <param name="rotation_threshold" value="0.2618"/>
      <param name="translation_threshold" value="0.4"/>
      <param name="tracking_rate_threshold" value="0.5"/>

      <!-- Feature optimization config -->
      <param name="feature/config/translation_threshold" value="-1.0"/>

      <!-- These values should be standard deviation -->
      <param name="noise/gyro" value="0.01"/>
      <param name="noise/acc" value="0.1"/>
      <param name="noise/gyro_bias" value="0.005"/>
      <param name="noise/acc_bias" value="0.05"/>
      <param name="noise/feature" value="0.03"/>

      <param name="initial_state/velocity/x" value="0.0"/>
      <param name="initial_state/velocity/y" value="0.0"/>
      <param name="initial_state/velocity/z" value="0.0"/>

      <!-- These values should be covariance -->
      <param name="initial_covariance/velocity" value="0.25"/>
      <param name="initial_covariance/gyro_bias" value="0.0001"/>
      <param name="initial_covariance/acc_bias" value="0.01"/>
      <param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
      <param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>

      <remap from="~imu" to="/zed2/zed_node/imu/data_raw"/>
      <remap from="~features" to="image_processor/features"/>

    </node>
  </group>

</launch>

image_processor_zed.launch

<launch>

  <arg name="robot" default="zed"/>
  <arg name="calibration_file"
    default="$(find msckf_vio)/config/camchain-imucam-zed.yaml"/>

  <!-- Image Processor Nodelet  -->
  <group ns="$(arg robot)">
    <node pkg="nodelet" type="nodelet" name="image_processor"
      args="standalone msckf_vio/ImageProcessorNodelet"
      output="screen">

      <rosparam command="load" file="$(arg calibration_file)"/>
      <param name="grid_row" value="4"/>
      <param name="grid_col" value="5"/>
      <param name="grid_min_feature_num" value="3"/>
      <param name="grid_max_feature_num" value="4"/>
      <param name="pyramid_levels" value="3"/>
      <param name="patch_size" value="15"/>
      <param name="fast_threshold" value="10"/>
      <param name="max_iteration" value="30"/>
      <param name="track_precision" value="0.01"/>
      <param name="ransac_threshold" value="3"/>
      <param name="stereo_threshold" value="5"/>

      <remap from="~imu" to="/zed2/zed_node/imu/data_raw"/>
      <remap from="~cam0_image" to="/zed2/zed_node/left_raw/image_raw_color"/>
      <remap from="~cam1_image" to="/zed2/zed_node/right_raw/image_raw_color"/>

    </node>
  </group>

</launch>

[ke-sun]

Cannot tell too much from the gif.

One thing I noticed that there was a significant lag between the movement in rviz and the images. I am not sure if this is an artifact of gif. Maybe the code was not compiled in release mode (there would be a lot of red prints on stdout if this is the issue)?

Based on the calibration file, it seems you calibrated the rectified images. But the subscribed topics were raw images. That could be the issue.

[truncs]

Thanks for the prompt reply! I recalibrated using raw iimages and yes, the code was compiled in release mode but I still run into similar issues. Made a video this time.

[ke-sun]

The red msgs on the terminal (as shown in the video) are indicators that the computation takes too long. If you are using an Intel CPU, it is probably because the code is compiled in debug mode. I suggest to recompile the whole workspace in release mode in order to be sure. If you are using devices like TX2 or Jetson nano, unfortunately, the code cannot easily run on these devices as it is.

[truncs]

@ke-sun It works well on my desktop! Any thoughts on how to improve the performance on TX2/Xavier NX like devices?

[ke-sun]

The first thing to try is to reduce the number of features. Maybe try reduce the grid_min_feature_num and grid_max_feature_num to 1 and 2.

[truncs]

That doesn't really help much. Would cuda compatible opencv help or is RANSAC the bottleneck? Is there a way to get a call graph with individual processing time?

[uts-hb]

Hi @truncs and @ke-sun , Sorry to bother you, I am just wondering about the result. I am also trying to use zed2 to simulate MSCKF. However, it's not really working well. Have you tried VINS-fusion too? If you did, Did you use the same transformation matrices or did you inverse it? When I saw the example of the euroc config file, it seems like they inversed it to get transformation matrix for MSCKF but other example does not. Also, even if I tried with all the different parameters, it doesn't seem like it's converging and showing the unsynced warning which is sometimes okay but still not converges. Looking forward to hearing from you. Thanks!

[ke-sun]

Hi @Hughbyun

I am not sure about VINS-fusion. But the frame convention of calibration used by this repository follows the EuRoC dataset (with some minor changes in removing brackets).

The unsync warning is a strong indicator that the calibration might be wrong.

[uts-hb]

Hi @ke-sun,

Thanks for the reply. I have fixed the problem of unsync warning by using the raw image instead of rect image. However, it seems like it converges at the beginning but after moving around for few seconds, it starts to diverge. Is it the problem of initial parameters? I tried with the few different parameters still not really working. Looking forward to hearing from you. Thanks a lot!

[ke-sun]

It's hard to tell what the problem is based on the description. Is there a short video of "it converges at the beginning but after moving around for few seconds, it starts to diverge"?

[uts-hb]

Really appreciate for your help. I reduce the grid_min_feature_num and grid_max_feature_num to 1 and 2 and I think it is working okay even though it sometimes diverge at the beginning. The attached video is the one I tried with grid_min_feature_num and grid_max_feature_num, 3 and 4, respectively. I can see that red message showing total processing time which I cannot see when the value was 1 and 2. Is it just a computational time problem?

https://user-images.githubusercontent.com/64269877/122861235-7221b700-d362-11eb-89b2-2f2061af545e.mp4

https://user-images.githubusercontent.com/64269877/122861783-5f5bb200-d363-11eb-9a4d-2826d9a83c26.mp4

[ke-sun]

If you are running the software on a desktop, the runtime warning shows up probably because the repository is not compiled in release mode.

[uts-hb]

Thank you @ke-sun. Yes, that video was testing with jetson NX. I cannot see that problem when I play the rosbag on my desktop. Thanks again for the help.

[kzamzam]

Hi @truncs, and @ke-sun , I am also trying to run my data taken with zed2 camera. I still didn't run kalibr, and was thinking of using the data defined by the config file for the zed2 right away, so I had 2 questions: 1- Your cam0_IMU is very different from mine, as mine is almost identity in rotation part, I got this from the topic published by the zed ros wrapper, left_cam_IMU transform, and convert the quaternion to transformation matrix. 2- In msckf_vio_zed.launch, how do you get the noise std and covariance? Thank you.