Skeleton Detection not working with Depth ! about hri_fullbody HOT 5 CLOSED

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

from hri_fullbody.

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 !

from hri_fullbody.

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.

from hri_fullbody.

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 !

from hri_fullbody.

Closing the issue as it should be fixed by #6

from hri_fullbody.