Skeleton Detection not working with Depth !

[AmirpooyaSh]

Hey there,

I was trying to use HRI Fullbody package to detect a human inside Gazebo Simulation. My sensor is Astra Camera (The same attached to Turtle Bot robot), so the gazebo plugin is the same. When I'm feeding the corresponding topics to the full_body launch file, it can recognize the human model and define the skeleton. However, when I'm trying to visualize it through Rviz, it seems that the publishing topic is not measuring x,y,z of the detected skeleton properly.

Here is my modified launch file:

Then, the Gazebo Simulation and Rviz visualization results are like below:

I haven't tested it using real Astra Camera yet. I assume that the real RGB-D sensor would make it work and it's more or less an issue related to the astra camera's Gazebo plugin (I also tested it using Xtion Gazebo Plugin, got into the same issue).

Here is also my astra.xacro file:

<?xml version="1.0"?>

true 20.0 ${70.0*M_PI/180.0} B8G8R8 640 480 0.4 15.0 camera/${name} true 10 ${name}/rgb/image_raw ${name}/depth/image_raw ${name}/depth/points ${name}/rgb/camera_info ${name}/depth/camera_info ${name} 0.1 0.0 0.0 0.0 0.0 0.0 0.4 8.0

I'd appreciate it if someone could help me with this problem.

[skadge]

hi! First, really cool to see hri_fullbody used in simulation!! well done!

We use a patch of pixels from the depth map (ie, not the point cloud) to estimate the depth. According to https://ros.org/reps/rep-0118.html , depth can be encoded either as a 16bit/milimeters or 32bits/meters.

Our code has been develop with the RealSense camera, that uses 16bits/milimeters, and does not check for 32bits/float (I assume your camera publishes in that format) -> the distances are divided by 1000, which cause the wrong distance estimation.

The fix should be fairly easy to implement, around that line https://github.com/ros4hri/hri_fullbody/blob/master/src/hri_fullbody/fullbody_detector.py#L741 and that line https://github.com/ros4hri/hri_fullbody/blob/master/src/hri_fullbody/rs_to_depth.py#L35, checking somewhere for the depth image format.

Do you feel like submitting a PR? it would be much appreaciated!

Do you

[AmirpooyaSh]

Hey @skadge,

As far as I understand the function "rgb_to_xyz" is trying to do the conversion and by just removing the division of z to 1000 the localization will get fixed.

The code below started to work on localizing a single skeleton correctly.

def rgb_to_xyz( x_rgb, y_rgb, rgb_camera_info, depth_camera_info, depth_data, roi_xmin = 0., roi_ymin = 0.): depth_model = PinholeCameraModel() rgb_model = PinholeCameraModel()

depth_model.fromCameraInfo(depth_camera_info)
rgb_model.fromCameraInfo(rgb_camera_info)

x_rgb = x_rgb + (roi_xmin * rgb_model.width)
y_rgb = y_rgb + (roi_ymin * rgb_model.height)

if x_rgb > rgb_model.width:
    x_rgb = rgb_model.width - 1
if y_rgb > rgb_model.height:
    y_rgb = rgb_model.height - 1

x_d = int(((x_rgb - rgb_model.cx())
           * depth_model.fx()
           / rgb_model.fx())
          + depth_model.cx())
y_d = int(((y_rgb - rgb_model.cy())
           * depth_model.fy()
           / rgb_model.fy())
          + depth_model.cy())
z = depth_data[y_d][x_d]
x = (x_d - depth_model.cx())*z/depth_model.fx()
y = (y_d - depth_model.cy())*z/depth_model.fy()

return np.array([x, y, z])

Before asking a PR for that, I'd like to double-check with another problem I face now. For single skeleton detection, everything is fine, but when I'm trying to use multiple skeleton detection (In the simulation), I get the below error:

[INFO] [1694110389.553657, 0.000000]: Using depth camera for body position estimation: True [INFO] [1694110389.554227, 0.000000]: Setting up for multibody pose estimation [INFO] [1694110389.554737, 0.000000]: Waiting for ids on /humans/bodies/tracked Exception in thread Thread-5: Traceback (most recent call last): File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner self.run() File "/opt/ros/noetic/lib/python3/dist-packages/rospy/timer.py", line 237, in run self._callback(TimerEvent(last_expected, last_real, current_expected, current_real, last_duration)) File "/tiago_public_ws/src/hri_fullbody/nodes/detect", line 100, in do_diagnostics KeyValue(key="Last detected body ID", value=str(next(reversed(self.detected_bodies)))), StopIteration

About a PR, yes why not ! However, I'm not that expert on working with GitHub, so not sure how can I do that :D !

[severin-lemaignan]

Regarding the change: the PR needs to handle both the 32bits encoding (ie, depth in meters) and the 16bits encoding (depth in mm).

For the other issue, please open a separate issue.

[severin-lemaignan]

Regarding how to create pull requests: have a look here: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/creating-a-pull-request !

[severin-lemaignan]

Closing the issue as it should be fixed by #6