Unclear transformations, possibly a bug

[TwardzikTomas]

Hello,
I am somewhat confused about your tracking messages. You provide 3 pose estimators, odometry, vo_pose and vo_pose_covariance, although, I cannot find out how these three differ. If I look at the messages with echo command in command line, they all give me same position. Naturally, vo_pose does not provide covariance. But in RVIZ at the beginning, odometry arrow points out of input_left_camera_frame and vo_pose and vo_pose_covariance arrow points out of base frame.

This can be seen from following picture:

However, in time, odometry arrow vanishes completely and vo_pose and vo_pose_covariance arrow slides elsewhere, where the topic echo messages show it is.

To showcase this behavior I am appending you echo output and rviz screen shot:

header:
stamp:
sec: 1655833101
nanosec: 443741631
frame_id: map
pose:
pose:
position:
x: -0.026482975110411644
y: 1.1688748598098755
z: -0.07859078794717789
orientation:
x: 0.06656732322526643
y: -0.0011859150693369661
z: -0.26026932096369937
w: 0.9632379164775666
covariance: [0.0015559596940875053, -1.4628109283876256e-06, 0.0003669109137263149, 7.136500789783895e-05, -0.002161735901609063, -0.0004413025744725019, -1.4623600463892217e-06, 0.0011519042309373617, 5.1062728744000196e-05, 0.0020212936215102673, 0.00013677265087608248, -0.0008255475549958646, 0.0003669107973109931, 5.106258686282672e-05, 0.000697773473802954, 0.0002427915169391781, -0.0004011623386759311, -0.0001721317821647972, 7.136560452636331e-05, 0.0020212936215102673, 0.00024279169156216085, 0.004053641576319933, 0.00015628525579813868, -0.00145142269320786, -0.0021617363672703505, 0.0001367734366795048, -0.0004011626588180661, 0.00015628631808795035, 0.003537524025887251, 0.0003253195609431714, -0.00044130286551080644, -0.0008255474385805428, -0.00017213186947628856, -0.0014514223439618945, 0.00032532005570828915, 0.0019263405120000243]

From the picture you can clearly see, that the vo_pose arrow reflects the values from echo output, but it is disconnected from the robot body. Also I would like to highlight the fact, that base link is very close to origin, as I have placed the camera back to its original position after some arbitrary moves. From this I gather that positioning works fine, however the outputs might be somehow confused, possibly my configuration is causing all this trouble. I would generally expect odometry to describe position of the base_link using SLAM and vo_poses to describe the same thing without the closing of the loop. Is my assumption correct? Why is the odometry arrow pointing out of the camera frame, while maintaining values of vo_pose topics?

My launch configuration is following:

`
import launch
def generate_launch_description():

visual_slam_node = ComposableNode(
    name='visual_slam_node',
    package='isaac_ros_visual_slam',
    plugin='isaac_ros::visual_slam::VisualSlamNode',
    parameters=[{
                'enable_rectified_pose': True,
                'enable_localization_n_mapping': True,
                'denoise_input_images': False,
                'rectified_images': True,
                'enable_debug_mode': False,
                'debug_dump_path': '/tmp/elbrus',
                'enable_slam_visualization': False,
                'enable_landmarks_view': False,
                'enable_observations_view': False,
                'enable_imu': True,
                'input_imu_frame': 'elbrus_mag_link',
                'map_frame': 'map',
                'odom_frame': 'elbrus_vis_odom',
                'base_frame': 'base_link2',
                'input_left_camera_frame': 'elbrus_left_camera_frame',
                'input_right_camera_frame': 'elbrus_right_camera_frame',
                'path_max_size': 1000
                }],
    remappings=[('stereo_camera/left/image', '/zed2/zed_node/left/image_rect_color'),
                ('stereo_camera/left/camera_info', '/zed2/zed_node/left/camera_info'),
                ('stereo_camera/right/image', '/zed2/zed_node/right/image_rect_color'),
                ('stereo_camera/right/camera_info', '/zed2/zed_node/right/camera_info'),
                ('visual_slam/imu', '/zed2/zed_node/imu/data')]
)

visual_slam_launch_container = ComposableNodeContainer(
    name='visual_slam_launch_container',
    namespace='',
    package='rclcpp_components',
    executable='component_container',
    composable_node_descriptions=[
        visual_slam_node
    ],
    output='screen'
)

return launch.LaunchDescription([visual_slam_launch_container])

`

I have static TF tree node, that builds my relation between base_link2 and input_camera_frames. Also I have found 'enable_rectified_pose': True in your launch files, however it is not documented anywhere. What is it's purpose?

Hardware setup

Nvidia Jetson Xavier AGX
Jetpack 4.6.1
Ubuntu 18.04
ROS2 Foxy
Elbrus version 10.0
ZED2 camera, ZED SDK 3.7
Docker image l4t-base:r32.7.1

[hemalshahNV]

The difference between odometry and vo_pose is that the former is "smooth" and has no jumps but can drift where as the latter is rectified and has "localization jumps" when closing loops. The flag "enable_rectified_pose" refers to this latter behavior.