KumarRobotics / msckf_vio

Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
Other
1.66k stars 592 forks source link

odom frame #133

Closed boeun98 closed 2 years ago

boeun98 commented 2 years ago

Can you please tell me from where the odom frame come in the bag file?

Currently, I'm using oCamS-1CGN-U camera and microstrain IMU with frame id of gx5_link.

and I can see the points in rviz but when I move the camera, the camera base frame do not move on rviz. . . . . .

https://user-images.githubusercontent.com/72862584/161984854-8091d4b7-b832-4fed-af1b-55f1826ba26c.mp4

. . . . .

Which fixed frame id and child frame id do I need to use instead of world and odom respectively? . . . . rostopic list : . rostopic list . . . .

_I don't know what I write the fixed frame id and child frame id in msckf_vioeuroc.launch file....

My camera's base link is ocams_base_link and my IMU's base link is gx5_link. . . . . Here's my modified files. . .

  1. msckf_vio_euroc.launch
<launch>

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

  <!-- Image Processor Nodelet  -->
  <include file="$(find msckf_vio)/launch/image_processor_euroc.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="20"/>
      <param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
      <param name="child_frame_id" value="gx5_link"/>
      <param name="max_cam_state_size" value="20"/>
      <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.005"/>
      <param name="noise/acc" value="0.05"/>
      <param name="noise/gyro_bias" value="0.001"/>
      <param name="noise/acc_bias" value="0.01"/>
      <param name="noise/feature" value="0.035"/>

      <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.01"/>
      <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="/gx5/imu/data"/>
      <remap from="~features" to="image_processor/features"/>

    </node>
  </group>

</launch>
  1. image_processor_euroc.launch
<launch>

  <arg name="robot" default="firefly_sbx"/>
  <arg name="calibration_file"
    default="$(find msckf_vio)/config/camchain-imucam-euroc.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="/gx5/imu/data"/>
      <remap from="~cam0_image" to="/stereo/left/image_raw"/>
      <remap from="~cam1_image" to="/stereo/right/image_raw"/>

    </node>
  </group>

</launch>

. . . . . My tf frame is show below. Screenshot from 2022-04-06 22-51-49 . . . . . . . . . . And I have more questions. .

  1. fixed frame id / child frame id before I wrote above.
  2. How can I check that external IMU data enters the program well?
  3. I understand the IMU state is related to the left camera and the Camera state is related to the right camera in the paper "Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight". It is right?
  4. Do the N camera states represent about the stereo camera or the right or left camera? . camera state
versatran01 commented 2 years ago
  1. I have answered the frame id questions in previous issues. You can set it to whatever you want.
  2. You can add a print in imuCallback
  3. No. Imu state is imu and camera state is the left camera
  4. left