local_orientation_per_joint always equal or close to [0,0,0,1]

haavarpb commented 2 years ago

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

Hi, title says it. Global orientation works, however local_orientation_per_joint seems not to work.

I have tried to look for similar issues, and I have checked my parameters for the camera as per the documentation, but I can't seem to get them to work.

Steps to Reproduce

Run this file in ros2.

from pyzed import sl
import __future__
from tf2_ros import TransformBroadcaster, TransformStamped
from geometry_msgs.msg import Quaternion
from rclpy.node import Node
import rclpy

class SkeletonTrackingCamera(sl.Camera, Node):
    """Wrapper class to sl.Camera which is configured for skeleton tracking."""

    def __init__(self):
        super().__init__(node_name="skeleton_tf_publisher")
        self.init_params = sl.InitParameters()
        self.init_params.camera_resolution = sl.RESOLUTION.HD1080  # Use HD720 video mode
        self.init_params.depth_mode = sl.DEPTH_MODE.ULTRA
        self.init_params.coordinate_units = sl.UNIT.METER
        self.init_params.sdk_verbose = True
        self.init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Z_UP

        self.object_detection_params = sl.ObjectDetectionParameters()
        self.object_detection_params.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_ACCURATE
        self.object_detection_params.enable_tracking = True
        self.object_detection_params.image_sync = True
        self.object_detection_params.enable_mask_output = False
        self.object_detection_params.enable_body_fitting = True
        self.object_detection_params.body_format = sl.BODY_FORMAT.POSE_34

        # Positional tracking module
        self.positional_tracking_params = sl.PositionalTrackingParameters()
        #self.positional_tracking_params.set_floor_as_origin = True

        # Object detection runtime parameters
        self.object_detection_runtime_params = sl.ObjectDetectionRuntimeParameters()
        self.object_detection_runtime_params.detection_confidence_threshold = 40

    def open_and_initialize(self) -> None:
        """Open camera, while also enabling the additional modules for skeleton tracking."""
        self._throw_if_not_success(super().open(self.init_params))
        self._initialize()

    def _initialize(self) -> None:
        # These calls require an opened camera
        self._throw_if_not_success(
            self.enable_positional_tracking(
                self.positional_tracking_params))
        self._throw_if_not_success(self.enable_object_detection(
            self.object_detection_params))

    def retrieve_objects(self, objects: sl.Objects) -> None:
        self._throw_if_not_success(super().retrieve_objects(
            objects, self.object_detection_runtime_params))

    def grab(self) -> bool:
        self._throw_if_not_success(super().grab())
        return True

    def _throw_if_not_success(self, err):
        if err != sl.ERROR_CODE.SUCCESS:
            raise CameraError(err)

class PosiredError(Exception):
    def __init__(self, message: str):
        Exception.__init__(self)
        self.message = message

    def __str__(self) -> str:
        return str(self.message)

class CameraError(PosiredError):
    def __init__(self, error_code: sl.ERROR_CODE):
        super().__init__(str(error_code))

def main(args=None):
    rclpy.init(args=args)
    node = SkeletonTrackingCamera()
    tf_broadcaster = TransformBroadcaster(node)

    node.open_and_initialize()
    objects = sl.Objects()
    while node.grab():
        node.retrieve_objects(objects)
        if objects.is_new:
            objects_list = objects.object_list
            if len(objects_list) > 0:
                first_object = objects_list[0]

                t = TransformStamped()
                t.header.frame_id = "camera_frame"
                t.child_frame_id = sl.BODY_PARTS_POSE_34.PELVIS.name
                t.transform.translation.x = float(first_object.keypoint[sl.get_idx_34(sl.BODY_PARTS_POSE_34.PELVIS)][0])
                t.transform.translation.y = float(first_object.keypoint[sl.get_idx_34(sl.BODY_PARTS_POSE_34.PELVIS)][1])
                t.transform.translation.z = float(first_object.keypoint[sl.get_idx_34(sl.BODY_PARTS_POSE_34.PELVIS)][2])
                q = Quaternion()
                q.x = first_object.global_root_orientation[0]
                q.y = first_object.global_root_orientation[1]
                q.z = first_object.global_root_orientation[2]
                q.w = first_object.global_root_orientation[3]
                t.transform.rotation = q
                tf_broadcaster.sendTransform(t)
                for joint in list(sl.BODY_PARTS_POSE_34):
                    if joint == sl.BODY_PARTS_POSE_34.LAST:
                        continue

                    parent_bone = [bone[0] for bone in sl.BODY_BONES_POSE_34 if bone[1] == joint]
                    if len(parent_bone) > 0:
                        t = TransformStamped()
                        t.header.frame_id = parent_bone[0].name
                        t.child_frame_id = joint.name
                        t.transform.translation.x = float(first_object.local_position_per_joint[sl.get_idx_34(joint)][0])
                        t.transform.translation.y = float(first_object.local_position_per_joint[sl.get_idx_34(joint)][1])
                        t.transform.translation.z = float(first_object.local_position_per_joint[sl.get_idx_34(joint)][2])
                        q = Quaternion()
                        node.get_logger().info(str(first_object.local_orientation_per_joint[sl.get_idx_34(joint)]))
                        q.x = float(first_object.local_orientation_per_joint[sl.get_idx_34(joint)][0])
                        q.y = float(first_object.local_orientation_per_joint[sl.get_idx_34(joint)][1])
                        q.z = float(first_object.local_orientation_per_joint[sl.get_idx_34(joint)][2])
                        q.w = float(first_object.local_orientation_per_joint[sl.get_idx_34(joint)][3])
                        tf_broadcaster.sendTransform(t)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

Expected Result

I expect getting local orientation per joint to change over time, not stay still.

Actual Result

Title.

ZED Camera model

ZED2

Environment

OS: Linux, Ubuntu 20.04
ZED SDK v3.7

Anything else?

No response

haavarpb commented 2 years ago

Hi, again.

I have now tried a different platform, using windows with the same issue. Still using pyzed 3.7 and CUDA 10.2 on this specific machine.

I'll paste a minimal example where I have modified your body tracking example python application. Observe that the local orientation per joint is [0,0,0,1] or very close.

########################################################################
#
# Copyright (c) 2021, STEREOLABS.
#
# All rights reserved.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
# OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
########################################################################

import pyzed.sl as sl
import numpy as np

def main():
    # Create a Camera object
    zed = sl.Camera()

    # Create a InitParameters object and set configuration parameters
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD720  # Use HD720 video mode
    init_params.depth_mode = sl.DEPTH_MODE.PERFORMANCE
    init_params.coordinate_units = sl.UNIT.METER
    init_params.sdk_verbose = True

    # Open the camera
    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    obj_param = sl.ObjectDetectionParameters()
    obj_param.enable_tracking = True
    obj_param.image_sync = True
    obj_param.enable_mask_output = True
    obj_param.enable_body_fitting = True
    obj_param.detection_model = sl.DETECTION_MODEL.HUMAN_BODY_ACCURATE
    obj_param.body_format = sl.BODY_FORMAT.POSE_34

    camera_infos = zed.get_camera_information()
    if obj_param.enable_tracking:
        positional_tracking_param = sl.PositionalTrackingParameters()
        # positional_tracking_param.set_as_static = True
        positional_tracking_param.set_floor_as_origin = True
        zed.enable_positional_tracking(positional_tracking_param)

    print("Object Detection: Loading Module...")

    err = zed.enable_object_detection(obj_param)
    if err != sl.ERROR_CODE.SUCCESS:
        print(repr(err))
        zed.close()
        exit(1)

    objects = sl.Objects()
    obj_runtime_param = sl.ObjectDetectionRuntimeParameters()
    obj_runtime_param.detection_confidence_threshold = 40

    while zed.grab() == sl.ERROR_CODE.SUCCESS:
        err = zed.retrieve_objects(objects, obj_runtime_param)
        if objects.is_new:
            obj_array = objects.object_list
            print(str(len(obj_array)) + " Object(s) detected\n")
            if len(obj_array) > 0:
                first_object = obj_array[0]
                print("First object attributes:")
                print(
                    " Label '"
                    + repr(first_object.label)
                    + "' (conf. "
                    + str(int(first_object.confidence))
                    + "/100)"
                )
                if obj_param.enable_tracking:
                    print(
                        " Tracking ID: "
                        + str(int(first_object.id))
                        + " tracking state: "
                        + repr(first_object.tracking_state)
                        + " / "
                        + repr(first_object.action_state)
                    )
                position = first_object.position
                velocity = first_object.velocity
                dimensions = first_object.dimensions
                print(
                    " 3D position: [{0},{1},{2}]\n Velocity: [{3},{4},{5}]\n 3D dimentions: [{6},{7},{8}]".format(
                        position[0],
                        position[1],
                        position[2],
                        velocity[0],
                        velocity[1],
                        velocity[2],
                        dimensions[0],
                        dimensions[1],
                        dimensions[2],
                    )
                )
                if first_object.mask.is_init():
                    print(" 2D mask available")

                print(" Bounding Box 2D ")
                bounding_box_2d = first_object.bounding_box_2d
                for it in bounding_box_2d:
                    print("    " + str(it), end="")
                print("\n Bounding Box 3D ")
                bounding_box = first_object.bounding_box
                for it in bounding_box:
                    print("    " + str(it), end="")

                orientations = first_object.local_orientation_per_joint
                for it in orientations:
                    print("   "+str(it), end='')

                input("\nPress enter to continue: ")

    # Close the camera
    zed.close()

if __name__ == "__main__":
    main()

github-actions[bot] commented 2 years ago

This issue is stale because it has been open 30 days with no activity. Remove stale label or comment otherwise it will be automatically closed in 5 days

PVLukas commented 1 year ago

Hi Håvard,

I have the same issue with my current project. Did you ever happen to find a solution?

CSE-JS commented 1 year ago

I have the same issue on Windows with SDK version 3.8.2, but only with spine bones.

PVLukas commented 1 year ago

I have the same issue on Windows with SDK version 3.8.2, but only with spine bones.

Late reply, but if you only got the problem on Spine 0, that's because they actually take the orientation of Spine 1, their graphic in the documentation is incorrect. I reported that to them in November 22 and assume it might be fixed in their new release already. In general, not all joints give a orientation per joint, Spine 3 is the only other spine joint that gives the rotation. Check this graphic: https://community.stereolabs.com/uploads/default/optimized/1X/589a51e1676063869f6464a2cddfd48faff587ba_2_518x500.jpeg

Jonny-air commented 1 year ago

I am using the 38 Keypoints and it looks to me as if, local_orientation_per_joint and local_position_per_joint are in fact relative transformations to their parent joint unlike the documentation suggests. At least that's what the data looks like for the hand joints. The wrist is at a constant distance x to the ellbow and rotates the hand coordinate system, in which the fingers are placed with zero rotation and a more or less constant position (at least for the index that's the case)

stereolabs / zed-python-api