How to guarantee tf last transformation in skeleton tracker

asked 2017-01-04 08:14:44 -0600

altella gravatar image

Hello all,

I am programming a gesture recognition node using the skeleton TF tree provided by the node openni_tracker (using the depth information of a Kinect at 30 FPS). Under certain conditions (slight body angles not facing perfectly the Kinect sensor) the tf output seems to be noisy and the algorithm fails to recognize the gestures. The original code to read the tf tree and construct the gesture vector is as follows:

int ReadTransform(std::string targetFrame, std::string sourceFrame, std::vector<double> &transformVector)
{ 
    tf::StampedTransform transform;

    transformVector.clear();
    try
    {
      listener->waitForTransform(targetFrame, sourceFrame,ros::Time(0),ros::Duration(0.3));
      listener->lookupTransform(targetFrame, sourceFrame, ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) 
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      return -1;
    }
    transformVector.push_back(transform.getOrigin().x());
    transformVector.push_back(transform.getOrigin().y());
    transformVector.push_back(transform.getOrigin().z()); 
    transformVector.push_back(transform.getRotation().x()); 
    transformVector.push_back(transform.getRotation().y()); 
    transformVector.push_back(transform.getRotation().z()); 
    transformVector.push_back(transform.getRotation().w()); 
    return 0;
}

int ReadTransforms(std::vector<double> &gestureVector)
{
    std::vector<double> transformVector;
    std::string user = boost::lexical_cast<std::string>(calibratedUser);

    gestureVector.clear();

    if(ReadTransform("left_hand_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("left_elbow_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("left_shoulder_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("neck_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("head_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("right_shoulder_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("right_elbow_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);
    if(ReadTransform("right_hand_" + user,"torso_" + user, transformVector) < 0) return -1;
    InsertPointToGesture(transformVector, gestureVector);

    return 0;
}

Pros: The return from "lookupTransform" is immediate, very fast. Cons: I do not have guaranteed the latest transformation. Could be possible to have many tf transformations queued and read tranforms occurred some seconds ago?

I have modified the listener part:

transformVector.clear();
    try
    {
      ros::TIme t = ros::Time::now()
      listener->waitForTransform(targetFrame, sourceFrame, t, ros::Duration(0.3));
      listener->lookupTransform(targetFrame, sourceFrame, t, transform);
    }

Pros: Last transform guaranteed. Cons: MUCH SLOWER (almost 0.5 s to perform eight transforms and construct the gesture vector)

Is there an alternative way to get the last transformation available in tf while increasing the speed? Thank you all very much,

Alberto

edit retag flag offensive close merge delete