Fusing two odoms (VO) using robot_localization

edit

Your tree should look like map->odom_filtered->base_link->... not base_link->odomX. Take the odometry message of source1 (I asume its camera centered, so frame_ID would be "camera1_base") fuse it in robot_localization ekf (link text with odom2 and output the fused data (ekf output) in odom_filtered frame.

@Dragonslayer Hi. I fell asleep before I could tag you and you beat me to it. So welcome to the post. I am starting from the basics. My config file for robot_localization is below. But I am still confused what needs to go in for odom_frame, base_link_frame, map_frame, world_frame ?

For the sake of simplicity, the frame names for camera1 are cam1_odom->cam1_base->cam1_optical... and the frame names for camera2 are cam2_odom->cam2_base->cam2_optical... and the PC is the base_link

frequency: 5
two_d_mode: true
publish_tf: true
predict_to_current_time: true
print_diagnostics: true

**odom_frame: ???
base_link_frame: ???
#map_frame: ???
world_frame: ???**

# xyz, rpy, vxyz, vrpy, axyz
odom0: /camera1/odom
odom0_config: [true,...
               ...false]
odom0_differential: false

odom1: /camera2/odom
odom1_config: [false,... 
               ..., false]
odom1_differential: true ...

• odom_frame: odom #published by ekf itslef.
• base_link_frame: base_link #the robot base link/chassis of your system. Or projected base_footprint frame depending on robot model. Published to tf from urdf robot model (usually) via robot_state_publisher.
• map_frame: map #The map frame the localization or slam package publishes.
• world_frame: odom #defaults to odom if unspecified for fusing continous data like in your case this is the way to go.

@Dragonslayer: I get the following warning messages repeatedly when I run the localization node with the frames set to

odom_frame: odom
base_link_frame: base_link
map_frame: map
world_frame: odom

Warnings:

[ WARN] [1599935167.107900670]: Could not obtain transform from cam2_base to base_link. Error was Could not find a connection between 'base_link' and 'cam2_base' because they are not part of the same tree.Tf has two or more unconnected trees.

[ WARN] [1599935169.111119120]: Could not obtain transform from cam1_odom to odom. Error was Could not find a connection between 'odom' and 'cam1_odom' because they are not part of the same tree.Tf has two or more unconnected trees.

...

In a way I would expect these errors because there are no tf which would establish a relation between cam1 tree, cam2 tree, and base_link. :/

You dont need the cam1 and cam2 tree. just a urdf for the robot model, base_link up to the frame the cams odometries are in reference to (camera_linkX or cam1_base or whatever its called).

@Dragonslayer So, I turned off the tf between the camX_odom --> camX_base. I then modified the robot urdf to publish base_link --> camX_base urdfs. Now my tf tree looks like this.

And now when I try to run robot_localization, I get the following warn message repeatedly:

[ WARN] [1599967026.507911316]: Could not obtain transform from cam1_odom to odom. Error was "cam1_odom" passed to lookupTransform argument source_frame does not exist

This I would expect too, since there is no tf to relate cam1_odom to any other frames. Do I need to manually publish the inverse tf from camX_base --> camX_odom?

Are you sure its ekf node that sends the warn? What does your /camera1/odom message look like (frame_ID)? Likely somewhere in your setup some node is looking for cam1_odom but your odom frame now is odom or odom_filtered whatever you put in the ekf parameter odom_frame. You dont get a cam2_odom warning?

@Dragonslayer The warning messages only start when I start the ekf node and in the same window where ekf is running. The odom_frame in ekf config is set to odom. Here is the config-file for ekf node. If I change the order of odom in config i.e. put odom0: /camera2/odom and odom1: /camera1/odom, the warning message change to cam2_odom warnings instead of cam1_odom warnings:

[ WARN] [1600008042.140231369]: Could not obtain transform from cam2_odom to odom. Error was "cam2_odom" passed to lookupTransform argument source_frame does not exist

This is what the /camera1/odom message looks like:

header: 
  seq: 1252
  stamp: 
    secs: 1600008164
    nsecs: 680465541
  frame_id: "cam1_odom"
child_frame_id: "cam1_base"
pose: 
  pose: 
    position: ...