void KinectTracker::updateSkeleton() {
assert(getSkeleton);
if(skelUser == 0)
return;
XnSkeletonJoint openNIJoints[Skeleton::MAX_JOINTS];
XnUInt16 numJoints = Skeleton::MAX_JOINTS;
xn::SkeletonCapability cap = userNode.GetSkeletonCap();
cap.EnumerateActiveJoints(openNIJoints, numJoints);
std::map<Skeleton::Joint, JointInfo>& joints = skel._getJointMap();
std::map<Skeleton::Joint, JointInfo>& joints2D = skel2D._getJointMap();
/* Nuke the maps from orbit. It's not the only way to be sure, and probably not
the most efficient thing to do, but it's easier than finding out which joints
have become inactive and deleting just those. */
joints.clear();
joints2D.clear();
for(int i = 0; i < numJoints; i++) {
XnSkeletonJointTransformation jointTransform;
cap.GetSkeletonJoint(skelUser, openNIJoints[i], jointTransform);
XnVector3D& openNIPosition = jointTransform.position.position;
const cv::Point3f position(openNIPosition.X, openNIPosition.Y, openNIPosition.Z);
depthNode.ConvertRealWorldToProjective(1, &openNIPosition, &openNIPosition);
const cv::Point3f pos2D(openNIPosition.X, openNIPosition.Y, 0.0f);
const float posConfidence = jointTransform.position.fConfidence;
const XnFloat* openNIRotation = jointTransform.orientation.orientation.elements;
cv::Mat rotation(3, 3, CV_32FC1);
// Both the OpenNI and the OpenCV matrices are stored in row-major order, so the
// following works out nicely
memcpy(rotation.data, openNIRotation, 9*sizeof(float));
const float rotConfidence = jointTransform.orientation.fConfidence;
/* Skeleton joints were taken from OpenNI, but OpenNI's start at 1,
while libobtrack's start at the C++ default of 0, hence the -1.
*/
const Skeleton::Joint curJoint = static_cast<Skeleton::Joint>(openNIJoints[i] - 1);
joints.insert(std::make_pair(curJoint,
JointInfo(position, posConfidence, rotation, rotConfidence)));
// TODO: Find a way to convert the rotations to 2D and pass them on
joints2D.insert(std::make_pair(curJoint,
JointInfo(pos2D, posConfidence, cv::Mat(), 0.0f)));
}
}
void ReferenceGenerator::setJointNames(const std::vector<std::string>& joint_names)
{
joint_names_ = joint_names;
// Create mapping from joint name to index
joint_name_to_index_.clear();
for(unsigned int i = 0; i < joint_names_.size(); ++i)
joint_name_to_index_[joint_names_[i]] = i;
joint_info_.resize(joint_names_.size(), JointInfo());
}
void ReferenceGenerator::initJoint(const std::string& name, double max_vel, double max_acc,
double min_pos, double max_pos)
{
int idx = this->joint_index(name);
if (idx < 0)
{
joint_name_to_index_[name] = joint_names_.size();
joint_names_.push_back(name);
idx = joint_info_.size();
joint_info_.push_back(JointInfo());
}
initJoint(idx, max_vel, max_acc, min_pos, max_pos);
}
