zed_aruco_localization outputting infinite transform

[dylan-gonzalez]

Preliminary Checks

• [X] This issue is not a duplicate. Before opening a new issue, please search existing issues.
• [X] This issue is not a question, feature request, or anything other than a bug report directly related to this project.

Description

I am trying to run the zed_aruco_localization example with just one marker, set to the origin of the map frame (i.e. pos = 0,0,0, or = 0,0,0).

But whenever I try to run this example with one marker, it outputs an infinite transform. I have tried this at different distances and doesn't seem to affect it.

When I try running the example with 2 markers, it doesn't output an infinite transform. But the transform is still quite off, and am just wondering if there is anything I am missing?

Here is my config file:

# config/aruco_loc.yaml
# Parameters for ArUco localization

---
/**:
    ros__parameters:

      general:
        marker_count: 1             # Number of markers in the World
        marker_size: 0.16           # Width/Height of the ArUco markers [m]
        maximum_distance: 3.0       # Maximum distance of the target from the camera to consider it valid
        detection_rate: 0.5         # Maximum detection frequency for pose update
        camera_name: 'zed'          # Name of the camera to relocate
        world_frame_id: 'map'       # Frame id of the world frame
        refine_detection: false     # If enabled the corners of the detected markers will be processed to obtain sub-pixel precision

      debug:
        active: false               # Enable debug messages

      marker_000:
        aruco_id: 40                 # ID of the ArUco tag as retrieved by the ArUco Detector code
        position: [0.0,0.0,0.0]
        orientation: [0.0,0.0,0.0]   # Orientation with respect to the World origin [rad]

      #marker_001:
      #  aruco_id: 19                 # ID of the ArUco tag as retrieved by the ArUco Detector code
      #  position: [0.0,0.0,1.09]     # Pose with respect to the World origin [m]
      #  orientation: [0.0,0.0,0.0]   # Orientation with respect to the World origin [rad]

      # Add a number of marker definitions equal to the number of markers in the World according to the `marker_count` value

      #marker_xxx:
      #  aruco_id: YYY               # ID of the ArUco tag as retrieved by the ArUco Detector code
      #  position: [x.x,y.y,z.z]     # Pose with respect to the World origin [m]
      #  orientation: [R.R,P.P,Y.Y]  # Orientation with respect to the World origin [rad]

Steps to Reproduce

1. ros2 launch zed_aruco_localization zed_aruco_loc.launch.py camera_model:=zed2i

Expected Result

Should be outputting a valid transform from the marker to the camera.

Actual Result

It outputs an infinite transformation.

ZED Camera model

ZED2i

Environment

OS: Ubuntu 22.04
CPU: AMD64 (also tried on ARM64 (Jetson Xavier NX))

ZED SDK 4.0.8
ROS2 Humble

Anything else?

No response

[Myzhar]

Check that the parameter marker_size: 0.16 is correct for the dimensions of your tag.

[dylan-gonzalez]

Check that the parameter marker_size: 0.16 is correct for the dimensions of your tag.

it is definitely correct.

[Myzhar]

Please record an SVO with ZED Explorer pointing your tag in the same conditions and share it with me so I can test the node as if I'm using your camera

[dylan-gonzalez]

Thanks @Myzhar I have tried again in a different environment, and it appears to be very accurate now.

[dylan-gonzalez]

Sorry @Myzhar , I don't think I looked too much at the output last night. When doing it again, it seems to detect the marker range very accurately (e.g. 1.56m), but the transformation it publishes is of very high numbers.

I have attached an image of the output. How should I share the SVO file with you? I can't attach it here.

[dylan-gonzalez]

I've just run it again and now it is working fine.

It seems random that sometimes it can't find the transform, and other times it can. I haven't changed anything about my setup, not sure what is going on here.

If it helps, I have set the marker coordinates to be the origin

I also get this error frequently: canTransform 'zed_left_camera_frame' -> 'zed_camera_link': Invalid frame ID "zed_left_camera_frame" passed to canTransform argument target_frame - frame does not exist.

[Myzhar]

Please record an SVO with ZED Explorer pointing your tag in the same conditions and share it with me so I can test the node as if I'm using your camera

This request is still pending

[dylan-gonzalez]

Sorry, as I mentioned before, it won't let me paste SVO files in here. How should I share it with you?

[Myzhar]

You can use the file-sharing service you prefer: Google Drive, Dropbox, etc.

[dylan-gonzalez]

Let me know if there are any issues accessing it here

[Myzhar]

The SVO that you linked does work correctly:

[component_container-3] [INFO] [1713433718.635480153] [zed.aruco_node]: *****************************
[component_container-3] [INFO] [1713433718.635526986] [zed.aruco_node]:     ArUco detection
[component_container-3] [INFO] [1713433718.636650542] [zed.aruco_node]:  * Color conversion: 0.0010955 sec
[component_container-3] [INFO] [1713433718.645467893] [zed.aruco_node]:  * Marker detection: 0.00876424 sec
[component_container-3] [INFO] [1713433718.645519226] [zed.aruco_node]:  * Detected tags: 2
[component_container-3] [INFO] [1713433718.645743926] [zed.aruco_node]:  * Marker poses estimation: 0.000219078 sec
[component_container-3] [INFO] [1713433718.645749342] [zed.aruco_node]:  * Nearest marker: 40 -> 1.65173m
[component_container-3] [INFO] [1713433718.645752655] [zed.aruco_node]:  * ArUco marker #40 in range: 1.65173 m
[component_container-3] [INFO] [1713433718.645758027] [zed.aruco_node]: pose_aruco -> Pos: [-0.134,0.055,1.645] - Or: [176.733°,-12.235°,-0.993°]
[component_container-3] [INFO] [1713433718.645766275] [zed.aruco_node]: pose_img -> Pos: [-0.217,-0.041,1.637] - Or: [176.443°,12.155°,178.278°]
[component_container-3] [INFO] [1713433718.645771754] [zed.aruco_node]: pose_marker -> Pos: [1.637,-0.217,-0.041] - Or: [1.686°,-3.193°,167.746°]
[component_container-3] [INFO] [1713433718.645776486] [zed.aruco_node]: pose_marker -> Pos: [1.639,-0.157,-0.057] - Or: [1.686°,-3.193°,167.746°]
[component_container-3] [INFO] [1713433718.645781577] [zed.aruco_node]: *** Calling ZED 'set_pose' service ***
[component_container-3] [INFO] [1713433718.645784919] [zed.aruco_node]:  * New camera pose [map]-> Pos:[1.639,-0.157,-0.057] Or:[1.686°,-3.193°,167.746°]
[component_container-3] [INFO] [1713433718.645890967] [zed.aruco_node]: [getTransformFromTf] canTransform 'zed_left_aruco' -> 'zed_base_aruco':. canTransform returned after 5.657e-06 timeout was 0.. canTransform returned after 8.573e-06 timeout was 1.
[component_container-3] [INFO] [1713433718.648987076] [zed.aruco_node]: [getTransformFromTf] 'zed_base_aruco' -> 'zed_left_aruco': 
[component_container-3]     [0.010,-0.060,-0.015] - [0.000°,-0.000°,0.000°]
[component_container-3] [INFO] [1713433718.649007615] [zed.aruco_node]: test TF -> Pos: [0.010,-0.060,-0.015] - Or: [1.686°,-3.193°,167.746°]
[component_container-3] [INFO] [1713433718.654633162] [zed.aruco_node]:  * Publish image result: 0.00561398 sec
[component_container-3] [INFO] [1713433718.654646086] [zed.aruco_node]: *****************************
[component_container-3] [INFO] [1713433718.656394743] [zed.zed_node]: ** Set Pose service called **
[component_container-3] [INFO] [1713433718.656406324] [zed.zed_node]: New pose: [1.63884,-0.156693,-0.057321, 0.0294222,-0.0557217,2.92772]
[component_container-3] [INFO] [1713433718.656410620] [zed.zed_node]: *** Starting Positional Tracking ***
[component_container-3] [INFO] [1713433718.656413999] [zed.zed_node]:  * Waiting for valid static transformations...
[component_container-3] [INFO] [1713433718.657517909] [zed.zed_node]: Initial ZED left camera pose (ZED pos. tracking): 
[component_container-3] [INFO] [1713433718.657533660] [zed.zed_node]:  * T: [1.63687,-0.217186,-0.0411453]
[component_container-3] [INFO] [1713433718.657541448] [zed.zed_node]:  * Q: [0.0292648,0.0116492,0.993838,0.106272]
[component_container-3] [INFO] [1713433718.661398840] [zed.aruco_node]:  * ZED Node replied to `set_pose` call: Positional Tracking new pose OK

Launch command: $ ros2 launch zed_aruco_localization zed_aruco_loc.launch.py camera_model:=zed2i svo_path:=~/Downloads/HD2K_SN39676143_22-13-37.svo

[dylan-gonzalez]

Thought it would. Sometimes I've been getting that infinite transform error like I stated before - any idea why?

[Myzhar]

I noticed that the tags are not perfectly "glued" to the wall so they are not perfectly planar. This can lead to unpredictable behaviors when reprojecting the points to the 3D plane.