void ofxKinectFeatures::computeJointDescriptors(int jointId, ofPoint jointPos, const float &h){
ofxMocapElement* mocapElement = getElement(jointId);
//Position
mocapElement->setPosition(jointPos);
//Filtered position
mocapElement->setPositionFiltered(applyFilter(mocapElement->getPosition(), mocapElement->getPositionFiltered(), aFilter, bFilter));
//Velocity
mocapElement->setVelocity(applyFilter(mocapElement->getPosition(), mocapElement->getVelocity(), aLpd1, bLpd1));
checkMaxAndMin(getVelocityHistory(jointId, 5), jointId, FEAT_VELOCITY);
//Acceleration
mocapElement->setAcceleration(applyFilter(mocapElement->getPosition(), mocapElement->getAcceleration(), aLpd2, bLpd2));
checkMaxAndMin(getAccelerationHistory(jointId, 5), jointId, FEAT_ACCELERATION);
//Acceleration along trajectory
ofPoint acc = mocapElement->getAcceleration()[0];
ofPoint vel = mocapElement->getVelocity()[0];
mocapElement->setAccelerationTrajectory(acc.dot(vel) / vel.length());
checkMaxAndMin(getAccelerationTrajectoryHistory(jointId, 5), jointId, FEAT_ACCELERATION_TRAJECTORY);
//Distance to torso
mocapElement->setDistanceToTorso(getPositionFiltered(jointId).distanceSquared(getPositionFiltered(torso_)));
checkMaxAndMin(getDistanceToTorsoHistory(jointId, 5), jointId, FEAT_DISTANCETOTORSO);
//Relative position to torso
//TODO solve this!!
ofPoint relPosToTorso;
relPosToTorso.x = (jointPos.x - getPositionFiltered(torso_).x) / (h * 1.8);
relPosToTorso.y = (jointPos.y - getPositionFiltered(torso_).y) / (h * 1.8);
relPosToTorso.z = -((jointPos.z - getPositionFiltered(torso_).z) / h) / 1.4;
mocapElement->setRelativePositionToTorso(relPosToTorso);
checkMaxAndMin(getRelativePositionToTorsoHistory(jointId, 5), jointId, FEAT_RELATIVEPOSTOTORSO);
//TODO: auto hand
//beatTracker.update(getAccTrVector(JOINT_RIGHT_HAND), get3DFiltPosVector(JOINT_RIGHT_HAND)[coord::Y]);
}
void OpenNIUser::computeJointDescriptors(int jointId, ofPoint jointPos, const float &headTorsoDist)
{
MocapElement* mocapElement = getElement(jointId);
//Position
mocapElement->setPosition(jointPos);
//Filtered position
mocapElement->setPositionFiltered(applyFilter(mocapElement->getPosition(), mocapElement->getPositionFiltered(), aFilter, bFilter));
//Velocity
mocapElement->setVelocity(applyFilter(mocapElement->getPosition(), mocapElement->getVelocity(), aLpd1, bLpd1));
mocapElement->setVelocityMagnitude(mocapElement->getVelocity()[0].length());
//Acceleration
mocapElement->setAcceleration(applyFilter(mocapElement->getPosition(), mocapElement->getAcceleration(), aLpd2, bLpd2));
mocapElement->setAccelerationMagnitude(mocapElement->getAcceleration()[0].length());
//Acceleration along trajectory
ofPoint acc = mocapElement->getAcceleration()[0];
ofPoint vel = mocapElement->getVelocity()[0];
mocapElement->setAccelerationTrajectory(acc.dot(vel) / vel.length());
//Distance to torso
mocapElement->setDistanceToTorso(getPositionFiltered(jointId).distanceSquared(getPositionFiltered(JOINT_TORSO)));
//Relative position to torso
ofPoint relPosToTorso;
relPosToTorso.x = (jointPos.x - getPositionFiltered(JOINT_TORSO).x) / (headTorsoDist * 1.8);
relPosToTorso.y = (jointPos.y - getPositionFiltered(JOINT_TORSO).y) / (headTorsoDist * 1.8);
relPosToTorso.z = -((jointPos.z - getPositionFiltered(JOINT_TORSO).z) / headTorsoDist) / 1.4;
mocapElement->setRelativePositionToTorso(relPosToTorso);
//TODO: auto hand
//beatTracker.update(getAccTrVector(JOINT_RIGHT_HAND), get3DFiltPosVector(JOINT_RIGHT_HAND)[coord::Y]);
}
void OpenNIUser::updateFeatures()
{
if (user->getJoint(JOINT_TORSO).getWorldPosition() == ofPoint(0,0,0)) return;
if (elements_.empty())
{
for (int j = 0; j < user->getNumJoints(); j++) {
MocapElement newElement(j, depth_);
elements_.push_back(newElement);
}
}
ofPoint headPos = user->getJoint(JOINT_HEAD).getWorldPosition();
ofPoint torsoPos = user->getJoint(JOINT_TORSO).getWorldPosition();
//for CI
float xMax = numeric_limits<float>::min();
float yMax = numeric_limits<float>::min();
float zMax = numeric_limits<float>::min();
float xMin = numeric_limits<float>::max();
float yMin = numeric_limits<float>::max();
float zMin = numeric_limits<float>::max();
float headTorsoDist = headPos.distance(torsoPos);
float meanVel = 0.0; //for qom
for (int j = 0; j < user->getNumJoints(); j++)
{
computeJointDescriptors(j, user->getJoint((Joint) j).getWorldPosition(), headTorsoDist);
//qom
meanVel += getVelocityMagnitude(j);
//ci
xMin = min(xMin, getPositionFiltered(j).x);
yMin = min(yMin, getPositionFiltered(j).y);
zMin = min(zMin, getPositionFiltered(j).z);
xMax = max(xMax, getPositionFiltered(j).x);
yMax = max(yMax, getPositionFiltered(j).y);
zMax = max(zMax, getPositionFiltered(j).z);
}
// Add position to history
if (meanVels_.size() <= depth_) {
meanVels_.insert(meanVels_.begin(), meanVel / user->getNumJoints());
}
if (meanVels_.size() > depth_) {
meanVels_.pop_back();
}
qom_ = accumulate(meanVels_.begin(), meanVels_.end(), 0.0) / (meanVels_.size());
ci_ = ( -4.0 + (( abs(xMax-xMin) + abs(yMax-yMin) + abs(zMax-zMin) ) / headTorsoDist) ) / 6.0;
symmetry_ = 1.0 - (0.5 * (abs(sqrt(getDistanceToTorso(JOINT_RIGHT_HAND))-sqrt(getDistanceToTorso(JOINT_LEFT_HAND))) + abs(sqrt(getDistanceToTorso(JOINT_RIGHT_ELBOW))-sqrt(getDistanceToTorso(JOINT_LEFT_ELBOW)))) / headTorsoDist);
yMaxHands_ = max(getRelativePositionToTorso(JOINT_RIGHT_HAND).y, getRelativePositionToTorso(JOINT_LEFT_HAND).y);
}
float ofxKinectFeatures::getAngle(int j1, int j2, int j3){
float d12 = getPositionFiltered(j1).distance(getPositionFiltered(j2));
float d13 = getPositionFiltered(j1).distance(getPositionFiltered(j3));
float d23 = getPositionFiltered(j2).distance(getPositionFiltered(j3));
return RAD_TO_DEG * acos(( -(d13*d13) + d23*d23 + d12*d12 ) / (2*d23*d12 ) ); //cos rule
}
void ofxKinectFeatures::update(map<int, ofPoint> joints){
//Initialize elements
if (elements_.empty()) {
for (map<int, ofPoint>::iterator it = joints.begin(); it != joints.end(); it++) {
ofxMocapElement newElement(it->first, depth_);
elements_.push_back(newElement);
}
}
//Compute descriptors
//Hard-coded way to check if skeleton (bSkeleton doesn't work) and new data (isNewDataAvailable doesn't work)
// if (user.getJoint((Joint)0).getWorldPosition() != ofPoint(0,0,0) &&
// user.getJoint((Joint)0).getWorldPosition() != getElement((Joint)0)->getPosition()[0] ) {
newValues_ = true;
//TODO solve this!!
ofPoint headPos = joints[head_];
ofPoint torsoPos = joints[torso_];
float h = headPos.distance(torsoPos);
float meanVel = 0.0; //for qom
//for CI
float xMax = numeric_limits<float>::min();
float yMax = numeric_limits<float>::min();
float zMax = numeric_limits<float>::min();
float xMin = numeric_limits<float>::max();
float yMin = numeric_limits<float>::max();
float zMin = numeric_limits<float>::max();
for (map<int, ofPoint>::iterator it = joints.begin(); it != joints.end(); it++) {
int j = it->first;
computeJointDescriptors(it->first, it->second, h);
//qom
meanVel += getVelocityMagnitude(j);
//ci
if (getPositionFiltered(j).x > xMax) {
xMax = getPositionFiltered(j).x;
}
if (getPositionFiltered(j).y > yMax) {
yMax = getPositionFiltered(j).y;
}
if (getPositionFiltered(j).z > zMax) {
zMax = getPositionFiltered(j).z;
}
if (getPositionFiltered(j).x < xMin) {
xMin = getPositionFiltered(j).x;
}
if (getPositionFiltered(j).y < yMin) {
yMin = getPositionFiltered(j).y;
}
if (getPositionFiltered(j).z < zMin) {
zMin = getPositionFiltered(j).z;
}
}
// Add position to history
if (meanVels_.size() <= depth_) {
meanVels_.insert(meanVels_.begin(), meanVel/joints.size());
}
// remove positions from history
if (meanVels_.size() > depth_) {
meanVels_.pop_back();
}
//qom_ = accumulate(meanVels_.begin(), meanVels_.end(), 0.0) / (meanVels_.size());
// QOM insert
if (qom_.size() <= depth_) {
qom_.insert(qom_.begin(), accumulate(meanVels_.begin(), meanVels_.end(), 0.0) / (meanVels_.size()));
}
// QOM delete
if (qom_.size() > depth_) {
qom_.pop_back();
}
checkMaxAndMin(getQomHistory(5), NO_JOINT, FEAT_QOM);
//ci_ = ( -4.0 + (( abs(xMax-xMin) + abs(yMax-yMin) + abs(zMax-zMin) ) / h) ) / 6.0;
if (ci_.size() <= depth_) {
ci_.insert(ci_.begin(), ( -4.0 + (( abs(xMax-xMin) + abs(yMax-yMin) + abs(zMax-zMin) ) / h) ) / 6.0);
}
// QOM delete
if (ci_.size() > depth_) {
ci_.pop_back();
}
checkMaxAndMin(getCIHistory(5), NO_JOINT, FEAT_CI);
//TODO solve this!!
// symmetry_ = 1.0 - (0.5 * (abs(sqrt(getDistanceToTorso(JOINT_RIGHT_HAND))-sqrt(getDistanceToTorso(JOINT_LEFT_HAND))) + abs(sqrt(getDistanceToTorso(JOINT_RIGHT_ELBOW))-sqrt(getDistanceToTorso(JOINT_LEFT_ELBOW)))) / h);
//symmetry_ = 0.0;
//TODO solve this!!
//yMaxHands_ = max(getRelativePositionToTorso(JOINT_RIGHT_HAND).y, getRelativePositionToTorso(JOINT_LEFT_HAND).y);
//yMaxHands_ = 0.0;
// } else {
// newValues_
// = false;
// }
}
