Project openni_tracker skeleton to Image Coordinates

asked 2014-04-08 05:31:16 -0500

xana
11 ●1 ●1 ●3

updated 2016-10-24 09:02:17 -0500

ngrennan
1 ●1 ●1

I'm using ROS to control a Kinect, and I need to project the skeleton points I get with openni_tracker to image coordinates. I found an answer about converting world coordinates to image coordinates (P_screen = I * P_world, where I is the 3x4 intrinsics matrix), however, the mapping I get seems incorrect.

An example of the /tf topic I have recorded is below:

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1385988109
        nsecs: 907731078
      frame_id: /skeleton/openni_depth_frame
    child_frame_id: /skeleton/head_4
    transform: 
      translation: 
        x: 1.97650313527
        y: 0.467346462985
        z: 0.124516482569
      rotation: 
        x: 0.320303175814
        y: 0.601755401028
        z: 0.604153236547
        w: 0.412668365236
---

Could you please help me with the needed transform to get the image coordinates (RGB topic: /camera/rgb/image_color) of the above point (the head of Kinect skeleton). The rest of the skeleton point have similar format.

Also, I want to point out, that I used the skeleton_markerspackage and the visualization of the joints of the skeleton seems correct. However, the problem is that I don't know how to project those points on the RGB image.

edit retag flag offensive close merge delete

add a comment

1

answered 2014-04-08 07:45:37 -0500

Stefan Kohlbrecher

24361 ●173 ●299 ●444 https://www.energy-rob...

updated 2014-04-08 07:46:26 -0500

Same advice as here applies in your case. Have a look at image_geometry, it should be straightforward to use. The Projecting a TF frame onto an image basically just has to be modified for your use case.

edit flag offensive delete link

add a comment

0

answered 2016-03-30 03:27:57 -0500

PKumars
92 ●12 ●17 ●23

Here is the Final answer where I'm extracting all the joint coordinates and then publishing them in each topic.

#include <ros ros.h=""> #include <tf transform_listener.h=""> #include <geometry_msgs twist.h="">

int main(int argc, char** argv) { ros::init(argc, argv, "my_skeleton_tf_listener");

ros::NodeHandle node;

// publisher declaration
ros::Publisher neck_joint = node.advertise<geometry_msgs::Point>("neck_joint", 1);
ros::Publisher head_joint = node.advertise<geometry_msgs::Point>("head_joint", 1);
ros::Publisher torso_joint = node.advertise<geometry_msgs::Point>("torso_joint", 1);
ros::Publisher left_shoulder_joint = node.advertise<geometry_msgs::Point>("left_shoulder_joint", 1);
ros::Publisher left_elbow_joint = node.advertise<geometry_msgs::Point>("left_elbow_joint", 1);
ros::Publisher left_hand_joint = node.advertise<geometry_msgs::Point>("left_hand_joint", 1);
ros::Publisher right_shoulder_joint = node.advertise<geometry_msgs::Point>("right_shoulder_joint", 1);
ros::Publisher right_elbow_joint = node.advertise<geometry_msgs::Point>("right_elbow_joint", 1);
ros::Publisher right_hand_joint = node.advertise<geometry_msgs::Point>("right_hand_joint", 1);
ros::Publisher left_hip_joint = node.advertise<geometry_msgs::Point>("left_hip_joint", 1);
ros::Publisher left_knee_joint = node.advertise<geometry_msgs::Point>("left_knee_joint", 1);
ros::Publisher left_foot_joint = node.advertise<geometry_msgs::Point>("left_foot_joint", 1);
ros::Publisher right_hip_joint = node.advertise<geometry_msgs::Point>("right_hip_joint", 1);
ros::Publisher right_knee_joint = node.advertise<geometry_msgs::Point>("right_knee_joint", 1);
ros::Publisher right_foot_joint = node.advertise<geometry_msgs::Point>("right_foot_joint", 1);

// listener 
tf::TransformListener listener;

ros::Rate rate(50.0); // frequency of operation

while (node.ok())
{
    // Transforms declared for each joint
    tf::StampedTransform transform_neck, transform_head, transform_torso, 
                            transform_left_shoulder, transform_left_elbow, transform_left_hand, 
                                transform_right_shoulder, transform_right_elbow, transform_right_hand, 
                                    transform_left_hip, transform_left_knee, transform_left_foot,
                                        transform_right_hip, transform_right_knee, transform_right_foot;
    try
    {
        // each joint frame to reference frame transforms 
        listener.lookupTransform("/neck_1", "/openni_depth_frame",ros::Time(0), transform_neck);
        listener.lookupTransform("/head_1", "/openni_depth_frame",ros::Time(0), transform_head);
        listener.lookupTransform("/torso_1", "/openni_depth_frame",ros::Time(0), transform_torso);
        listener.lookupTransform("/left_shoulder_1", "/openni_depth_frame",ros::Time(0), transform_left_shoulder);
        listener.lookupTransform("/left_elbow_1", "/openni_depth_frame",ros::Time(0), transform_left_elbow);
        listener.lookupTransform("/left_hand_1", "/openni_depth_frame",ros::Time(0), transform_left_hand);
        listener.lookupTransform("/right_shoulder_1", "/openni_depth_frame",ros::Time(0), transform_right_shoulder);
        listener.lookupTransform("/right_elbow_1", "/openni_depth_frame",ros::Time(0), transform_right_elbow);
        listener.lookupTransform("/right_hand_1", "/openni_depth_frame",ros::Time(0), transform_right_hand);
        listener.lookupTransform("/left_hip_1", "/openni_depth_frame",ros::Time(0), transform_left_hip);
        listener.lookupTransform("/left_knee_1", "/openni_depth_frame",ros::Time(0), transform_left_knee);
        listener.lookupTransform("/left_foot_1", "/openni_depth_frame",ros::Time(0), transform_left_foot);
        listener.lookupTransform("/right_hip_1", "/openni_depth_frame",ros::Time(0), transform_right_hip);
        listener.lookupTransform("/right_knee_1", "/openni_depth_frame",ros::Time(0), transform_right_knee);
        listener.lookupTransform("/right_foot_1", "/openni_depth_frame",ros::Time(0), transform_right_foot);

    }
        catch (tf::TransformException &ex) 
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(0.10).sleep();
        continue;
    }

    // geometry points declaration for storing 3D coordinates of joints and then published later 
    geometry_msgs::Point neck_pose, head_pose, torso_pose, 
                            left_shoulder_pose, left_elbow_pose, left_hand_pose,
                                right_shoulder_pose, right_elbow_pose, right_hand_pose, 
                                    left_hip_pose, left_knee_pose, left_foot_pose, 
                                        right_hip_pose, right_knee_pose, right_foot_pose;

    // joint position extraction and store
    // neck joint                                   
    neck_pose.x = transform_neck.getOrigin().x();
    neck_pose.y = transform_neck.getOrigin().y();
    neck_pose.z = transform_neck.getOrigin().z();
    // head joint
    head_pose.x = transform_head.getOrigin().x();
    head_pose.y = transform_head.getOrigin().y();
    head_pose.z = transform_head.getOrigin().z();
    // torso joint
    torso_pose.x = transform_torso.getOrigin().x();
    torso_pose.y = transform_torso.getOrigin().y();
    torso_pose.z = transform_torso.getOrigin().z();
    // left shoulder joint 
    left_shoulder_pose.x = transform_left_shoulder.getOrigin().x();
    left_shoulder_pose.y = transform_left_shoulder.getOrigin().y();
    left_shoulder_pose.z = transform_left_shoulder.getOrigin().z();
    // left elbow joint
    left_elbow_pose.x = transform_left_elbow.getOrigin().x();
    left_elbow_pose.y = transform_left_elbow.getOrigin().y();
    left_elbow_pose.z = transform_left_elbow.getOrigin().z();
    // left hand joint
    left_hand_pose.x = transform_left_hand.getOrigin().x();
    left_hand_pose.y = transform_left_hand.getOrigin().y();
    left_hand_pose.z = transform_left_hand ...

(more)