Can't use depth image to do pose estimation

[Phymin]

Hi, I'm using D415 to do pose estimation of aruco marker, but if I set 'use_depth' to true, there is no x and y data for the pose estimation, do you know why? below are two screenshots taken from my testing. Thanks

[SimonGiampy]

Hi, thanks for your feedback. Sorry for the delayed response but I was in vacation.
Can you be more specific in what you mean by

missing x and y data for the pose estimation

This feature of using the depth map for aruco pose estimation was experimental, so it may not work perfectly well with different cameras other than the D435 that I personally used. But the python code for this estimation using the depth map is commented, so you should be able to change it according to your configuration and obtain the missing values that you are seeking. The output is a tvec vector ( x, y, z coordinates), which is also printed out on the console.

[Phymin]

Hi, thanks for your response, "missing x and y data for the pose estimation" means the x and y data estimated from depth image is all 0, which it should not be. I know the tvec vector is also printed, but it's estimated from color image. What I want is also the estimation from depth image, so I can check which one is more accurate.

Do you have any suggestions to get the pose from depth image, in what ways I can change the code? Thansk

[kawata-yuya]

Hi, bro. my english is not very good, so I use a translator.

---

@Phymin @SimonGiampy

I had this problem too.
But in my case, even the value of z was missing, as shown here.
[aruco_node]: depthcloud centroid = [0. 0. 0.]

Reproduction Procedure

1. launch aruco_pose_estimation.launch.py .
2. Use option marker_size:=0.8 use_depth_input:=true .
3. Markers are detected, but [aruco_node]: depthcloud centroid = [0. 0. 0.] is always output

Used version

• Commit hash: 4eec132
• Branch: main
• Environment: Ubuntu 22.04, Python 3.10.12, ros2 humble, zsh
• real Sense: D435

Expected behavior

For example, outputs
[aruco_node]: depthcloud centroid = [-0.12 0.13 0.56]

launch and outputs

$ ros2 launch aruco_pose_estimation aruco_pose_estimation.launch.py marker_size:=0.8 use_depth_input:=true 

[INFO] [launch]: All log files can be found below /home/(blanked by me)
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [aruco_node.py-1]: process started with pid [49412]
[INFO] [realsense2_camera_node-2]: process started with pid [49414]
[INFO] [rviz2-3]: process started with pid [49416]
[rviz2-3] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[realsense2_camera_node-2] [INFO] [1735870324.809459148] [camera.camera]: RealSense ROS v4.51.1
[realsense2_camera_node-2] [INFO] [1735870324.809612763] [camera.camera]: Built with LibRealSense v2.55.1
[realsense2_camera_node-2] [INFO] [1735870324.809628432] [camera.camera]: Running with LibRealSense v2.55.1
[realsense2_camera_node-2] [INFO] [1735870324.870545366] [camera.camera]: Device with serial number 819312070263 was found.
[realsense2_camera_node-2] 
[realsense2_camera_node-2] [INFO] [1735870324.870642747] [camera.camera]: Device with physical ID /sys/devices/pci0000:00/0000:00:08.1/0000:04:00.4/usb4/4-2/4-2:1.0/video4linux/video6 was found.
[realsense2_camera_node-2] [INFO] [1735870324.870653236] [camera.camera]: Device with name Intel RealSense D435 was found.
[realsense2_camera_node-2] [INFO] [1735870324.870846876] [camera.camera]: Device with port number 4-2 was found.
[realsense2_camera_node-2] [INFO] [1735870324.870859058] [camera.camera]: Device USB type: 3.2
[realsense2_camera_node-2] [INFO] [1735870324.912672259] [camera.camera]: getParameters...
[realsense2_camera_node-2] [INFO] [1735870324.913006008] [camera.camera]: JSON file is not provided
[realsense2_camera_node-2] [INFO] [1735870324.913031926] [camera.camera]: Device Name: Intel RealSense D435
[realsense2_camera_node-2] [INFO] [1735870324.913040562] [camera.camera]: Device Serial No: 819312070263
[realsense2_camera_node-2] [INFO] [1735870324.913047796] [camera.camera]: Device physical port: /sys/devices/pci0000:00/0000:00:08.1/0000:04:00.4/usb4/4-2/4-2:1.0/video4linux/video6
[realsense2_camera_node-2] [INFO] [1735870324.913055249] [camera.camera]: Device FW version: 5.12.15.50
[realsense2_camera_node-2] [INFO] [1735870324.913062623] [camera.camera]: Device Product ID: 0x0B07
[realsense2_camera_node-2] [INFO] [1735870324.913069386] [camera.camera]: Sync Mode: On
[realsense2_camera_node-2] [INFO] [1735870324.925634752] [camera.camera]: Set ROS param depth_module.profile to default: 848x480x30
[realsense2_camera_node-2] [INFO] [1735870324.930379770] [camera.camera]: Set ROS param rgb_camera.profile to default: 640x480x30
[realsense2_camera_node-2] [INFO] [1735870324.951632433] [camera.camera]: Stopping Sensor: Stereo Module
[realsense2_camera_node-2] [INFO] [1735870324.977411668] [camera.camera]: Starting Sensor: Stereo Module
[realsense2_camera_node-2] [INFO] [1735870324.984225687] [camera.camera]: Open profile: stream_type: Depth(0), Format: Z16, Width: 848, Height: 480, FPS: 30
[realsense2_camera_node-2] [INFO] [1735870324.984542896] [camera.camera]: Stopping Sensor: RGB Camera
[realsense2_camera_node-2] [INFO] [1735870324.998245133] [camera.camera]: Starting Sensor: RGB Camera
[realsense2_camera_node-2] [INFO] [1735870325.077160889] [camera.camera]: Open profile: stream_type: Color(0), Format: RGB8, Width: 640, Height: 480, FPS: 30
[realsense2_camera_node-2] [INFO] [1735870325.077984438] [camera.camera]: RealSense Node Is Up!
[realsense2_camera_node-2] [WARN] [1735870325.141716939] [camera.camera]: 
[rviz2-3] [INFO] [1735870325.208527634] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1735870325.208648438] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-3] [INFO] [1735870325.225543652] [rviz2]: Stereo is NOT SUPPORTED
[aruco_node.py-1] [INFO] [1735870325.343990222] [aruco_node]: Marker size: 0.8
[aruco_node.py-1] [INFO] [1735870325.344597469] [aruco_node]: Marker type: DICT_4X4_50
[aruco_node.py-1] [INFO] [1735870325.345167718] [aruco_node]: Use depth input: True
[aruco_node.py-1] [INFO] [1735870325.345802917] [aruco_node]: Input image topic: /camera/color/image_raw
[aruco_node.py-1] [INFO] [1735870325.346370941] [aruco_node]: Input depth image topic: /camera/aligned_depth_to_color/image_raw
[aruco_node.py-1] [INFO] [1735870325.346897719] [aruco_node]: Image camera info topic: /camera/color/camera_info
[aruco_node.py-1] [INFO] [1735870325.347424537] [aruco_node]: Camera frame: camera_color_optical_frame
[rviz2-3] [INFO] [1735870325.406337601] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1735870325.411811623] [rviz2]: Stereo is NOT SUPPORTED
[aruco_node.py-1] [INFO] [1735870325.576781377] [aruco_node]: Camera info received.
[aruco_node.py-1] [INFO] [1735870325.577963141] [aruco_node]: Intrinsic matrix: [[617.49017334   0.         320.80661011]
[aruco_node.py-1]  [  0.         617.89593506 236.81387329]
[aruco_node.py-1]  [  0.           0.           1.        ]]
[aruco_node.py-1] [INFO] [1735870325.578907064] [aruco_node]: Distortion coefficients: [0. 0. 0. 0. 0.]
[aruco_node.py-1] [INFO] [1735870325.579448479] [aruco_node]: Camera frame: 640x480
[aruco_node.py-1] [INFO] [1735870326.047322586] [aruco_node]: depthcloud centroid = [0. 0. 0.]
[aruco_node.py-1] [INFO] [1735870326.050161536] [aruco_node]: tvec = [1.09004406] [0.00432544] [4.45657531]
[aruco_node.py-1] [INFO] [1735870326.382810919] [aruco_node]: depthcloud centroid = [0. 0. 0.]
[aruco_node.py-1] [INFO] [1735870326.384141569] [aruco_node]: tvec = [1.09004406] [0.00432544] [4.45657531]
[aruco_node.py-1] [INFO] [1735870326.673444815] [aruco_node]: depthcloud centroid = [0. 0. 0.]
[aruco_node.py-1] [INFO] [1735870326.674680980] [aruco_node]: tvec = [1.09004406] [0.00432544] [4.45657531]
[aruco_node.py-1] [INFO] [1735870326.968107490] [aruco_node]: depthcloud centroid = [0. 0. 0.]
[aruco_node.py-1] [INFO] [1735870326.969423102] [aruco_node]: tvec = [1.09004406] [0.00432544] [4.45657531]
[aruco_node.py-1] [INFO] [1735870327.290368260] [aruco_node]: depthcloud centroid = [0. 0. 0.]
[aruco_node.py-1] [INFO] [1735870327.291398633] [aruco_node]: tvec = [1.09004406] [0.00432544] [4.45657531]
...

[kawata-yuya]

solution

1. Open aruco_pose_estimation/aruco_pose_estimation/pose_estimation.py in your cloned repo .
2. Change L.220 to the following.

-     centroid = np.mean(np.array(pointcloud, dtype=np.uint16), axis=0)
+     centroid = np.mean(np.array(pointcloud, dtype=np.float32), axis=0)

cause

The elements of pointcloud were float, but when converted to np.array, they were cast to unsigned int16, so the decimal point was truncated.

[Phymin]

@kawata-yuya oh, thanks a lot