Parameter `input_base_frame` not working: base -> camera transform not taken into account?

[GuillaumeLaine]

Hi, first of all many thanks for sharing this great repository! Unfortunately I am having some issues obtaining odometry messages that account for the transformation between the robot and camera coordinate frames.

I am currently working with a ground robot and trying to track its position using a Realsense camera positioned on top of the robot, looking slightly down as so:

From the documentation, I seem to understand that your implementation is designed to allow tracking successive poses of the robot (i.e. frame base_link), despite computing these poses in the camera frame (i.e. frame front_camera_link). Namely, the Visual Odometry node parameter input_base_frame seems to be responsible for this:

input_base_frame: Name of the frame (baselink) to calculate transformation between the baselink and the left camera. Default is empty, which means the value of the base_frame will be used. If input_base_frame and base_frame are both empty, the left camera is assumed to be in the robot's center.

However, by launching the node with 'input_base_frame': 'base_link' as follows:

    visual_slam_node = ComposableNode(
        name='visual_slam_node',
        namespace='front_camera',
        package='isaac_ros_visual_slam',
        plugin='isaac_ros::visual_slam::VisualSlamNode',
        parameters=[{
                    'enable_rectified_pose': True,
                    'denoise_input_images': False,
                    'rectified_images': True,
                    'enable_debug_mode': False,
                    'debug_dump_path': '/tmp/elbrus',
                    'enable_slam_visualization': True,
                    'enable_landmarks_view': True,
                    'enable_observations_view': True,
                    'publish_tf': False,
                    'map_frame': 'map',
                    'odom_frame': 'odom',
                    'base_frame': 'base_link',
                    'input_base_frame': 'base_link',
                    'input_left_camera_frame': 'front_camera_infra1_frame',
                    'input_right_camera_frame': 'front_camera_infra2_frame',
                    }],
        remappings=[('stereo_camera/left/image', 'infra1/image_rect_raw'),
                    ('stereo_camera/left/camera_info', 'infra1/camera_info'),
                    ('stereo_camera/right/image', 'infra2/image_rect_raw'),
                    ('stereo_camera/right/camera_info', 'infra2/camera_info')]
    )

the camera position with respect to the robot frame does not seem to be taken into account. As a matter of fact, unless I am mistaken it seems that the ROS parameter input_base_frame is not used at all in the source code.

Here is an example where I start the VO tracking, and move the hand-held camera straight forward (positive X direction in the front_camera_link frame): Overlooking the jitter caused by my hand motion, I would expect the odometry arrows to be directed "into to ground" / co-linear to the front_camera_frame's X axis. Instead, the arrows seem to be directed along the X axis of the base_link frame i.e. the VO node considers the camera to be position at the robot's center?

Have I misunderstood the usage of the node's parameters or am I overlooking something? How can I track the motion of the robot instead of the camera's? Any help would be greatly appreciate! Thanks in advance.

[hemalshahNV]

Isaac ROS Visual SLAM will report odometry in the base_link frame. If you want to track the camera pose, leave both input_base_frame and base_frame are both empty and the left camera will be assumed to be the robot's "center."

[tanelikor]

I am having a problem and findings similar to what @GuillaumeLaine described.

Note: I am using version 0.9.3-ea3 because we are running ROS2 Galactic (we have not yet migrated to Humble). If this issue has been fixed in a later version (which I am currently unable to test without creating a separate test setup), I would greatly appreciate any instructions on how to backport the fix into a galactic-compatible version.

Here's my situation: We have a robot with 4 Realsense cameras (front, back and both sides), all looking in a horizontal direction. The current goal is to run the Visual Slam nodes for the two side cameras and get the odometries all reported for the movement of the base_link frame. Here are the settings that I'm using for the visual slam nodes:

visual_slam_node = ComposableNode(
    name=f"visual_slam_node",
    namespace=f"camera_{camera_count}",
    package="isaac_ros_visual_slam",
    plugin="isaac_ros::visual_slam::VisualSlamNode",
    extra_arguments=[{"use_intra_process_comms": True}],
    parameters=[
        {
            "enable_rectified_pose": True,
            "denoise_input_images": False,
            "rectified_images": True,
            "enable_debug_mode": False,
            "debug_dump_path": f"/tmp/elbrus_{camera_count}",
            "enable_slam_visualization": True,
            "enable_landmarks_view": True,
            "enable_observations_view": True,
            "publish_tf": False,
            "map_frame": "map",
            "odom_frame": "odom",
            "base_frame": "base_link",  
            "input_base_frame": "base_link",
            "input_left_camera_frame": f"camera_{camera_count}_infra1_frame",
            "input_right_camera_frame": f"camera_{camera_count}_infra2_frame",
        }
    ],
    remappings=[
        ("stereo_camera/left/image", "infra1/image_rect_raw"),
        ("stereo_camera/left/camera_info", "infra1/camera_info"),
        ("stereo_camera/right/image", "infra2/image_rect_raw"),
        ("stereo_camera/right/camera_info", "infra2/camera_info"),
    ],
)

In my first setup tests, I am driving the robot straight forward for a distance of about 3 meters (within accuracy of about 5-10 cm). The expected result is that both visual odometries would report about a 3m movement forward (in the positive x-direction of the base_link frame). Instead, what I'm seeing is this, white being wheel-odometry for reference - it is fairly accurate, green being odometry from the left camera and red from the right camera (it also seems that the visual slam nodes report distances of roughly half of the true one, but that I expect to be a separate issue that might also be due to our camera settings etc.):

I have also ran the test with several other frame configurations for the two base frames, such as

"base_frame": "",  
"input_base_frame": "",

"base_frame": f"camera_{camera_count}_link",  
"input_base_frame": "",

and

"base_frame": "base_link",  
"input_base_frame": f"camera_{camera_count}_link",

with them all giving the same output for odometry poses. Thus, it seems clear to me that the pose in the odometry output of the visual slam node is not correctly transformed to reflect the movement of the base_link frame but rather always represents the movement of (likely) the input_left_camera_frame.

I did find out that the twist part of the odometry does seem to be correctly oriented and that the frame settings do affect to it. However, it does seem (based on some other tests) that there is something wrong in the translation part of the twist (when driving in a curve, the camera towards the turn gives shorter distance than the one on the outside of the curve), but unfortunately I do not have an easily visualizable dataset to clearly show this.

I also tried running the node for the front camera, and for that the odometry pose path looks more reasonable, with the exception of the length of path (yellow poses are from the front camera):

@GuillaumeLaine Did you find a solution to your problem? I am considering a temporary workaround by writing a separate wrapper or node etc. to just do the necessary transformations "manually" before using the odometry, but would appreciate any alternatives if you have found a fix.

[swapnesh-wani-nvidia]

@GuillaumeLaine and @tanelikor, I have looked into your launch files, and it looks like you have "publish_tf": False. Could you try with "publish_tf": True and keep the input_base_frame: "base_link" ?

[tanelikor]

@swapnesh-wani-nvidia Thank you for the reply and suggestion. I tried it out, even though I was quite convinced that setting "publish_tf": True would not solve the issue. More thoughts on this later, first the results.

The launch config of the visual slam nodes now looks like this:

visual_slam_node = ComposableNode(
    name=f"visual_slam_node",
    namespace=f"camera_{camera_count}",
    package="isaac_ros_visual_slam",
    plugin="isaac_ros::visual_slam::VisualSlamNode",
    extra_arguments=[{"use_intra_process_comms": True}],
    parameters=[
        {
            "enable_rectified_pose": True,
            "denoise_input_images": False,
            "rectified_images": True,
            "enable_debug_mode": False,
            "debug_dump_path": f"/tmp/elbrus_{camera_count}",
            "enable_slam_visualization": True,
            "enable_landmarks_view": True,
            "enable_observations_view": True,
            "publish_tf": True,
            "map_frame": "map",
            "odom_frame": "odom",
            "base_frame": "base_link", 
            "input_base_frame": "base_link",
            "input_left_camera_frame": f"camera_{camera_count}_infra1_frame",
            "input_right_camera_frame": f"camera_{camera_count}_infra2_frame",
        }
    ],
    remappings=[
        ("stereo_camera/left/image", "infra1/image_rect_raw"),
        ("stereo_camera/left/camera_info", "infra1/camera_info"),
        ("stereo_camera/right/image", "infra2/image_rect_raw"),
        ("stereo_camera/right/camera_info", "infra2/camera_info"),
    ],
)

The result looks like this. (As a side note, the speed/distance mismatch was indeed due to bad camera configs but that I have since fixed).

Here's also a video about the drive.

https://user-images.githubusercontent.com/81684354/191219915-4693273a-7a5d-4b4f-8ed3-7c86d3b0e6a9.mp4

I then also tried this with "input_base_frame": f"camera_{camera_count}_link" with the following result.

https://user-images.githubusercontent.com/81684354/191220138-f6632384-fb60-4f70-875a-fc8cbe0f0eb5.mp4

If I understand correctly (and please correct me if I'm mistaken), setting "publish_tf": True simply makes the node publish the transform between odom and base_link frames, but doesn't affect to any other operations (transforms to the odometry data etc) done by the visual slam nodes. In my application, I definitely do not want to publish tfs from the visual slam nodes: Since I'm running two visual slam nodes and a node that publishes wheel-based odometry, publishing tf's from multiple nodes will create competing transforms and potentially cause oscillation (even if the odometries are transformed correctly), which can also be seen happening on the videos above. The actual end goal for me is to fuse all the odom sources and publish tfs based on the fused odometry.

[Parosi]

Hello,

I have the same problem. Did you solve it?

Thanks.

[tanelikor]

@Parosi As a temporary solution until a proper fix is made, I ended up just writing a separate node that

• looks up the transform between the camera and robot base frames,
• transforms the odometry data (including covariances) given by the visual slam nodes to describe the movement of the robot frame instead of that of the cameras, and
• relays the new odometry data to a separate topic that we then use elsewhere.

Its not an ideal solution as it does add some extra overhead, but so far it has been good enough for our purposes.

[Parosi]

@Parosi As a temporary solution until a proper fix is made, I ended up just writing a separate node that

* looks up the transform between the camera and robot base frames,

* transforms the odometry data (including covariances) given by the visual slam nodes to describe the movement of the robot frame instead of that of the cameras, and

* relays the new odometry data to a separate topic that we then use elsewhere.

Its not an ideal solution as it does add some extra overhead, but so far it has been good enough for our purposes.

Thanks @tanelikor , I was also thinking something like that. Did you publish the code for the additional node somewhere? This is a big problem and having a working node to fix it would be very appreciated!

[tanelikor]

@Parosi We have not published it because it is meant as a short-time temporary fix/workaround. The full version is also only in Python for the same reason (has been good enough for now).

However, we could make a public version of it as there seems to be a need for it. This will require a small amount of work to remove dependencies to our internal utilities, but I think I could get it done next week or so. So if there is interest in it, I'll make it happen. Could also then maybe post a link to it here if that is allowed, or just let you know about it otherwise.

[Parosi]

@tanelikor Yes I think it will be very useful and it can help many developers interested in this package!

I'm not sure, but I think it is possible to link here the repo with the fix.

Thank you very much!

[tanelikor]

@Parosi Alright, I'll let you know when the public version of the node is ready.

[tanelikor]

@Parosi The workaround is now published. I didn't find any mention of not being allowed to link different repos here, so you can find it at https://github.com/Karelics/odom_transformer.

[Parosi]

@tanelikor I will test it tomorrow or next week. Thank you very much!

[tuch77731]

I have same problem my config is 'base_frame': 'base_link', 'input_base_frame': 'camera_link'

it is not nature when rotation.

[pimientofrito]

Hi, I am having the same problem with Isaac 2.0. Has there been any news about this issue?

[tanelikor]

Hi @pimientofrito and @tuch77731 (apologies, I had totally missed your comment).

On our side, it seems like this was fixed at some point in the Humble version since migrating to that a couple months ago fixed the issue for us and we haven't needed the self-crafted transforming since. I'm not sure why there hasn't been any reports by the maintainers about that in this issue. I had plans to take some recording of this and post it here, but haven't found the time to do that thus far.

If you use a version where this has been fixed, then you should likely set the frames to be

"base_frame": "base_link",
"input_base_frame": "base_link"  # or ""

If you use a version where this has not been fixed (I do not know which ones those are exactly and it seems really difficult to decipher that from the release / update notes of the package) and wish to use the odom_transformer package that I linked above, then you should probably set both base_frame and input_base_frame to the value of the left camera frame. This could be "camera_link" or "camera_infra1_frame" etc, depending on which cameras you are using.

I hope this helps.

[pimientofrito]

Thanks for your quick response @tanelikor! I'm using a corrected version, if I'm not mistaken. In fact, I just became aware that the pose is indeed transforming to base_frame, but I noticed it in one particular case.

I'm testing visual odometry with real drone flights (from before takeoff to landing) and a setup of two pairs of cameras, one looking forward and one looking back with a -45 degree pitch turn (looking slightly down), my problem is with this one. If I launch the odometry with the camera looking backwards, the trajectory comes out rotated -45 degrees, this is why I thought it was not published in base_link. But if I launch it once the drone is flying and there is no movement in altitude, the estimated trajectory is what I expected.

I'm not sure if I should open another issue for this, but is this a normal behavior? is it possible that the takeoff is affecting the estimation?

[tanelikor]

@pimientofrito Sounds odd to me. If you have the frames set correctly and the necessary transforms are available, the transposing should work regardless of when you launch the node.

One point that does come to mind is the following: In our application, the cameras can sometimes have a highly obstructed view, which I have found to routinely cause the position estimate of the vslam node to go crazy (makes sense if the camera can't see anything). The relative movement of estimte does get fixed as soon as the visibility returns though, and I haven't seen it to cause problems with transforming to the base frame. Also, the position/orientation doesn't matter for us as we only use the velocity part of the odometry in sensor fusion with other sources that are not affected by visibility, so this is not a problem in our case.

So, if the camera is pointing to the ground and close to it and you rely on the pose information given by vslam, you might want to investigate the effect of the camera not seeing properly at launch. But if the issue is of the type that the base frame (drone) is reported to move backwards (in its own frame) when it is in reality moving forward, then I cannot quickly think of a sensible reason for this. Also, if it works fine with the front camera but not with the rear one, then it sounds like something that could relate to this issue (or at least it shows similar symptoms).

If you cannot find the reason for the problems, its probably a good idea to share some screenshots etc about how the problem looks like, either in this issue or in a separate one. At least I find it difficult to be exactly sure what the issue looks like based on just a text form description. :slightly_smiling_face:

[pimientofrito]

@tanelikor Great, thank you for your feedback. In the video you will be able to see the behavior of the odometry. This should give a clearer idea of the problem I'm encountering. First, I launch both odometries, and at some point I restart the one with the camera pointing backwards (i.e., kill the node and relaunch it).

The frames I'm using are:

                    'map_frame': 'map',
                    'odom_frame': 'odom',
                    "base_frame": "base_link",
                    "input_base_frame": "base_link",
                    'input_imu_frame': 'camera_rs_back_gyro_optical_frame',
                    'input_left_camera_frame': 'camera_rs_back_infra1_frame',
                    # 'input_right_camera_frame': '',

                    'map_frame': 'map',
                    'odom_frame': 'odom',
                    "base_frame": "base_link",
                    "input_base_frame": "base_link",
                    'input_imu_frame': 'camera_rs_front_gyro_optical_frame',
                    'input_left_camera_frame': 'camera_rs_front_infra1_frame',
                    # 'input_right_camera_frame': '',

TFs are being published by the RealSense node, and the link with base is like this:

    base2frontsensor = Node(
        package = "tf2_ros",
        executable = "static_transform_publisher",
        arguments = ["0.18", "0.005", "-0.035",
                    "0", "0", "0", "1",
                    "base_link", "camera_rs_front_link"])

    base2backsensor = Node(
        package = "tf2_ros",
        executable = "static_transform_publisher",
        arguments = ["-0.18", "-0.005", "-0.035",
                    "-0.3826834", "0", "0.9238795", "0",
                    "base_link", "camera_rs_back_link"]
    )

blue: front camera red: back camera https://github.com/NVIDIA-ISAAC-ROS/isaac_ros_visual_slam/assets/56796302/8e793730-9906-458e-9e80-dc9114b86d18

It is true that it starts very close to the ground, so I understand that this is a problem. Thank you again!

[tanelikor]

@pimientofrito Thanks for the video. To me this, looks like it is clearly an issue with the camera being blocked when the drone is on the ground. Observations that point to this:

• The movement direction seems correct in both odometries: when the drone flies forward, the odom arrows progress forward (in the direction of the arrow) etc, so the velocity information looks correct. This suggests that the camera transforms are correctly accounted for.
• Rviz displays large covariance for the pose estimate (the purple sphere for position and the yellow cones for orientation) at the beginning. This is likely from the rear camera not seeing properly (you might want to check in Rviz whether the large covariance is only from the rear camera or from both). Might be that the ground is closer than the minimum detection distance of the camera (check you camera specs for this) or close to it, or that the floor texture is not clear enough for vslam, combination of the two or something else.

Thus, it looks like the orientation of the rear vslam just gets messed up at the start when the camera doesn't see well.

The way to solve this depends on what you want to use the vslam for. If you want (combined) odometry from both cameras, I would suggest looking into some sensor fusion and utilizing only the velocity data at least from the worse camera. If you want them for slam, it might be better to just use the front camera.

[pimientofrito]

@tanelikor I take note of your impressions. Thank you very much for your patience and advice :smile:

[tuch77731]

Hi @pimientofrito and @tuch77731 (apologies, I had totally missed your comment).

On our side, it seems like this was fixed at some point in the Humble version since migrating to that a couple months ago fixed the issue for us and we haven't needed the self-crafted transforming since. I'm not sure why there hasn't been any reports by the maintainers about that in this issue. I had plans to take some recording of this and post it here, but haven't found the time to do that thus far.

If you use a version where this has been fixed, then you should likely set the frames to be

"base_frame": "base_link",
"input_base_frame": "base_link"  # or ""

If you use a version where this has not been fixed (I do not know which ones those are exactly and it seems really difficult to decipher that from the release / update notes of the package) and wish to use the odom_transformer package that I linked above, then you should probably set both base_frame and input_base_frame to the value of the left camera frame. This could be "camera_link" or "camera_infra1_frame" etc, depending on which cameras you are using.

I hope this helps.

It's very helpful to me! Thank you!

[swapnesh-wani-nvidia]

Thank you all for the active discussion. This is a long discussion. Could you folks try with the latest release of Isaac ROS.

[tanelikor]

@swapnesh-wani-nvidia Sorry for the late reply. I will try it out next week if I can spare the time.

[tanelikor]

Hi @swapnesh-wani-nvidia. It took me a bit longer than anticipated due to a very busy December, but I finally got to test this with version 2.1.0, and it seems to work as it should. So, as far as I'm concerned this issue can be closed.

I did find a different issue in that the new version does not work with intra-process communication, but I will create a separate issue about that.

[lowellm91]

I have been trying to solve a similar issue, it's not clear to me what the difference between "input_base_frame" and "left_camera_frame" since in the documentation both do the same thing(transform the base frame to the left_camera_frame). TF2 Names I'm assuming refer to the child.