Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Robot_localization: fuse absolute with relative measurements

I want to fuse global pose from tag pose estimation with visual odometry and IMU.

I am using:

  • a RealSense D435i with realsense-ros that provides all the transformations between each sensor
  • and rtab_map for the visual odometry

Fusing relative measurements (visual odometry and IMU) looks correct with rviz:

ekf_local.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: odom

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true]
odom0_differential: false
odom0_relative: true

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

I am not able to fuse absolute pose with visual odometry and IMU. I want to use something like this:

ekf_global.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: map

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true, true, true]

odom0_differential: false
odom0_relative: true

odom1: /tag_pose

odom1_config: [true,  true,  true,
               true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false]

odom1_differential: false
odom1_relative: false

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

odom0 is the output of rtab_map:

frame_id: "odom"
child_frame_id: "camera_link"

imu0 is:

frame_id: "camera_imu_optical_frame"

(Realsense ROS publishes all the transformations between the camera_link and for instance camera_imu_optical_frame.)

What should be the transformation for odom1?

Is it the transformation between the map (frame_id) and the camera_link (so the raw transformation between the tag and the camera)? Is it the transformation between the map (frame_id) and the odom? Since I have another ekf_node that fuses the continuous data (vo and IMU), I can transform the raw tag pose.

The output of the local ekf is clear. It is the transformation between the odom and the camera_link. The odom frame is the initial position of the sensor when launching the nodes.

What is the output of the global ekf (/ekf_global/odometry/filtered)? From the doc it should be the transformation between the map and odom. But from my understanding:

  • map frame is fixed and corresponds in my case to the tag frame
  • odom frame is the initial pose of the sensor and the transformation map / odom should be fixed
  • camera_link is the free floating camera frame

So does the ekf (/ekf_global/odometry/filtered) estimates the map --> odom transformation and I have to use the /ekf_local/odometry/filtered to get the transformation between the tag/map and the camera_link?

Robot_localization: fuse absolute with relative measurements

I want to fuse global pose from tag pose estimation with visual odometry and IMU.

I am using:

  • a RealSense D435i with realsense-ros that provides all the transformations between each sensor
  • and rtab_map for the visual odometry

Fusing relative measurements (visual odometry and IMU) looks correct with rviz:

ekf_local.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: odom

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true]
odom0_differential: false
odom0_relative: true

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

I am not able to fuse absolute pose with visual odometry and IMU. I want to use something like this:

ekf_global.yaml:

frequency: 30
sensor_timeout: 0.1
publish_tf: true
map_frame: map
odom_frame: odom
base_link_frame: camera_link
world_frame: map

odom0: /odom
odom0_config: [true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true,  true,  true,
               true, true, true]

odom0_differential: false
odom0_relative: true

odom1: /tag_pose

odom1_config: [true,  true,  true,
               true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false]

odom1_differential: false
odom1_relative: false

imu0: /imu/data
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]

imu0_differential: true
imu0_relative: false
imu0_remove_gravitational_acceleration: true

odom0 is the output of rtab_map:

frame_id: "odom"
child_frame_id: "camera_link"

imu0 is:

frame_id: "camera_imu_optical_frame"

(Realsense ROS publishes all the transformations between the camera_link and for instance camera_imu_optical_frame.)

What should be the transformation for odom1?

Is it the transformation between the map (frame_id) and the camera_link (so the raw transformation between the tag and the camera)? Is it the transformation between the map (frame_id) and the odom? Since I have another ekf_node that fuses the continuous data (vo and IMU), I can transform the raw tag pose.

The output of the local ekf is clear. It is the transformation between the odom and the camera_link. The odom frame is the initial position of the sensor when launching the nodes.

What is the output of the global ekf (/ekf_global/odometry/filtered)? From the doc it should be the transformation between the map and odom. But from my understanding:

  • map frame is fixed and corresponds in my case to the tag frame
  • odom frame is the initial pose of the sensor and the transformation map / odom should be fixed
  • camera_link is the free floating camera frame

So does the ekf (/ekf_global/odometry/filtered) estimates the map --> odom transformation and I have to use the /ekf_local/odometry/filtered to get the transformation between the tag/map and the camera_link?


Edit:

I did some tests, recapped here.

The homogeneous transformation a_T_b means: X_a = a_T_b . X_b.

The vision library detects the tag and estimates the transformation camlink_T_tag. The goal was to still have the transformation camlink_T_tag when the tag is not detected (occlusion, ...).

Option 1:

  • map and tag frames are the same
  • ekf_local/odometry/filtered estimates odom_T_camlink
  • ekf_global/odometry/filtered estimates map_T_camlink
  • when the tag is detected, the transformation map_T_camlink is published
  • when the tag is not detected, you have to publish the transformation map_T_camlink using the fixed transformation map_T_odom
  • ekf_global/odometry/filtered is used to get the filtered tag pose
  • result

Option 2:

  • map and odom frames are the same, you have to publish a static identity transform map_T_odom
  • ekf_local/odometry/filtered estimates odom_T_camlink
  • ekf_global/odometry/filtered estimates map_T_camlink
  • so both nodes (kind of) estimate the same transformation
  • when the tag is detected, the transformation map_T_camlink = map_T_tag . tag_T_camlink is published with map_T_tag being a static transform computed at the first tag detection
  • there is nothing to publish when the tag is not detected since map and odom are the same
  • but to have the transformation camlink_T_tag, you need the static transform tag_T_map to be published
  • result

Option 3:

  • filter only relative measurements
  • ekf_local/odometry/filtered estimates odom_T_camlink
  • when the tag is detected, you use the raw pose from the vision library
  • when the tag is not detected, you have camlink_T_tag = camlink_T_odom . odom_T_tag with odom_T_tag being a static transformation that must be computed when the tag is detected and with the output of ekf_local/odometry/filtered
  • result

I had not the time to test the three approaches on the same sequence, so this is just to have an idea.