void FubiRecorder::recordNextSkeletonFrame(const FubiUser& user, const Fubi::FingerCountData& leftFingerCountData, const Fubi::FingerCountData& rightFingerCountData)
{
const Fubi::TrackingData* data = m_useFilteredData
? user.currentFilteredTrackingData()
: user.currentTrackingData();
int leftFingerCount = -1, rightFingerCount = -1;
if (leftFingerCountData.trackingEnabled && leftFingerCountData.fingerCounts.size() > 0)
leftFingerCount = leftFingerCountData.fingerCounts.back();
if (rightFingerCountData.trackingEnabled && rightFingerCountData.fingerCounts.size() > 0)
rightFingerCount = rightFingerCountData.fingerCounts.back();
recordNextSkeletonFrame(data, leftFingerCount, rightFingerCount);
}
void FubiOpenNISensor::getSkeletonJointData(unsigned int id, Fubi::SkeletonJoint::Joint joint, Fubi::SkeletonJointPosition& position, Fubi::SkeletonJointOrientation& orientation)
{
FubiUser* user = Fubi::getUser(id);
if (user)
{
if (user->isTracked())
{
xn::SkeletonCapability cap = m_UserGenerator.GetSkeletonCap();
// Standard case user is tracked
// Get the current tracking data
XnSkeletonJoint xjoint = JointToXNSkeletonJoint(joint);
if (xjoint == XN_SKEL_WAIST) // special case currently not supported by OpenNI but approximated as center between hips
{
// Position is middle between the hips
XnSkeletonJointPosition pos1, pos2;
cap.GetSkeletonJointPosition(user->id(), XN_SKEL_LEFT_HIP, pos1);
cap.GetSkeletonJointPosition(user->id(), XN_SKEL_RIGHT_HIP, pos2);
position.m_confidence = minf(pos1.fConfidence, pos2.fConfidence);
position.m_position = xnJointToVec3f(pos2) + (xnJointToVec3f(pos1) - xnJointToVec3f(pos2))*0.5f;
// Rotation is the same as torso rotation
XnSkeletonJointOrientation rot;
cap.GetSkeletonJointOrientation(user->id(), XN_SKEL_TORSO, rot);
orientation = SkeletonJointOrientation(rot.orientation.elements, rot.fConfidence);
}
else
{
// Standard case, just get the data
XnSkeletonJointPosition pos;
XnSkeletonJointOrientation rot;
cap.GetSkeletonJointPosition(user->id(), xjoint, pos);
cap.GetSkeletonJointOrientation(user->id(), xjoint, rot);
position.m_position = xnJointToVec3f(pos);
position.m_confidence = pos.fConfidence;
orientation.m_orientation = Matrix3f(rot.orientation.elements);
orientation.m_confidence = pos.fConfidence;
}
}
else
{
// Not tracked return the center of mass instead
// that should be independent of tracking state
XnPoint3D com;
m_UserGenerator.GetCoM(user->id(), com);
position.m_confidence = 0.5f; // leave confidence at 0.5 as this is not really the wanted position
position.m_position.x = com.X;
position.m_position.y = com.Y;
position.m_position.z = com.Z;
}
}
}
void FubiRecorder::recordNextSkeletonFrame(const FubiUser& user, const FubiUser::FingerCountData& leftFingerCountData, const FubiUser::FingerCountData& rightFingerCountData)
{
if (m_isRecording && (user.currentTrackingData()->timeStamp - m_lastTimeStamp) > Math::Epsilon)
{
const FubiUser::TrackingData* data = m_useFilteredData
? user.currentFilteredTrackingData()
: user.currentTrackingData();
m_lastTimeStamp = data->timeStamp;
xml_document<> doc;
// Create base node
xml_node<>* frameNode = doc.allocate_node(node_element, "Frame");
doc.append_node(frameNode);
// With time attribute
xml_attribute<>* attr = doc.allocate_attribute("time", doc.allocate_string(numToString(m_lastTimeStamp-m_recordingStart, 6).c_str()));
frameNode->append_attribute(attr);
// And frame id
attr = doc.allocate_attribute("frameID", doc.allocate_string(numToString(m_currentFrameID++, 0).c_str()));
frameNode->append_attribute(attr);
// Create all sub nodes for the frame
xml_node<>* jointPositions = doc.allocate_node(node_element, "JointPositions");
frameNode->append_node(jointPositions);
xml_node<>* jointOrientations = doc.allocate_node(node_element, "JointOrientations");
frameNode->append_node(jointOrientations);
// Now fill them with the joint data
for (int i = 0; i < SkeletonJoint::NUM_JOINTS; ++i)
{
xml_node<>* joint = doc.allocate_node(node_element, "Joint");
joint->append_attribute(doc.allocate_attribute("name", getJointName((SkeletonJoint::Joint)i)));
const Vec3f* pos = &(data->jointPositions[i].m_position);
joint->append_attribute(doc.allocate_attribute("x", doc.allocate_string(numToString(pos->x, 0).c_str())));
joint->append_attribute(doc.allocate_attribute("y", doc.allocate_string(numToString(pos->y, 0).c_str())));
joint->append_attribute(doc.allocate_attribute("z", doc.allocate_string(numToString(pos->z, 0).c_str())));
joint->append_attribute(doc.allocate_attribute("confidence", doc.allocate_string(numToString(data->jointPositions[i].m_confidence, 0).c_str())));
jointPositions->append_node(joint);
joint = doc.allocate_node(node_element, "Joint");
joint->append_attribute(doc.allocate_attribute("name", getJointName((SkeletonJoint::Joint)i)));
const Vec3f* rot = &(data->jointOrientations[i].m_orientation.getRot());
joint->append_attribute(doc.allocate_attribute("x", doc.allocate_string(numToString(rot->x, 2).c_str())));
joint->append_attribute(doc.allocate_attribute("y", doc.allocate_string(numToString(rot->y, 2).c_str())));
joint->append_attribute(doc.allocate_attribute("z", doc.allocate_string(numToString(rot->z, 2).c_str())));
joint->append_attribute(doc.allocate_attribute("confidence", doc.allocate_string(numToString(data->jointOrientations[i].m_confidence, 0).c_str())));
jointOrientations->append_node(joint);
}
int leftFingerCount = -1, rightFingerCount = -1;
if (leftFingerCountData.trackingEnabled && leftFingerCountData.fingerCounts.size() > 0)
leftFingerCount = leftFingerCountData.fingerCounts.back();
if (rightFingerCountData.trackingEnabled && rightFingerCountData.fingerCounts.size() > 0)
rightFingerCount = rightFingerCountData.fingerCounts.back();
if (leftFingerCount > -1 || rightFingerCount > -1)
{
xml_node<>* fingercounts = doc.allocate_node(node_element, "FingerCounts");
frameNode->append_node(fingercounts);
fingercounts->append_attribute(doc.allocate_attribute("left", doc.allocate_string(numToString(leftFingerCount, 0).c_str())));
fingercounts->append_attribute(doc.allocate_attribute("right", doc.allocate_string(numToString(rightFingerCount, 0).c_str())));
}
// Write the frame node to the file
// Call the internal print_node directly to start at a different indent level
internal::print_node(std::ostream_iterator<char>(m_file), frameNode, 0, 1);
if (m_file.fail())
{
Fubi_logErr("Error writing skeleton data to file: %s \n", m_fileName.c_str());
stopRecording();
}
}
}
