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); }