Network System Laboratory - Backup diff of SLKinect1/sourcecode/convertRot2SLData(No. 3)

View the diff current.
View the source.
Go to SLKinect1/sourcecode/convertRot2SLData.

--- 2: 2011-11-11 (Fri) 15:34:38 iseki
+++ 3: 2011-12-11 (Sun) 21:25:50 iseki
@@ Line 1: / Line 1: @@
-*** convertRot2SLData (of SLKinect-v1.0.1) [#c6a2d789]
+ void    CExKinectWnd::convertRot2SLData(int uid)
- void    CExKinectWnd::convertRot2SLData(int uid)
  {
-     for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
+    Vector<double> vect;
-         int n = OpenNI2SLJointNum[j];
+    bool normal_joint[OPENNI_MAX_JOINT_NUM];
-         if (n>=0) {
-             XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
+    //
+    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
+        if (OpenNI2SLJointNum[j]>;=0) {
+            if (jointPositionConfidence((XnSkeletonJoint)j)>EXKINECT_WND_CONFIDENCE) {
+                XnVector3D  pos = jointPositionData((XnSkeletonJoint)j);
+                posVect[j].set(-pos.Z, pos.X, pos.Y);
+                prvVect[j] = posVect[j];
+            }
+            else {
+                posVect[j] = prvVect[j];
+            }
+        }
+    }
+    // mPelvis
+    posVect[0] = (posVect[17] + posVect[21])*0.5;
+    vect = posVect[21] - posVect[17];
+    double thz = atan2(-vect.x, vect.y);
+    rotQuat[0].setRotation(thz, 0.0, 0.0, 1.0);
+    if (!isCalibrated) setStartPosData(posVect[0]);
-             double m11 = rot.elements[0];
+    //
-             double m12 = rot.elements[1];
+    for (int j=1; j&lt;OPENNI_MAX_JOINT_NUM; j++) {
-&nbsp;            double m13 = rot.elements[2];
+        normal_joint[j] = true;
-             double m21 = rot.elements[3];
-             double m31 = rot.elements[6];
-             double m32 = rot.elements[7];
-             double m33 = rot.elements[8];
-             Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33);
-             Vector<double> vct;
-             if (j==3) {
+        if (OpenNI2SLJointNum[j]>=0) {
-                 vct.set(0.0, -eul.y/2, 0.0);
+            if (jointRotationConfidence((XnSkeletonJoint)j)>;EXKINECT_WND_CONFIDENCE) {
-                 rotQuat[0].setEulerYZX(vct);
+                XnMatrix3X3 rot = jointRotationData((XnSkeletonJoint)j);
-                 vct.set(-eul.x, -eul.y/2, eul.z);
+                double m11 = rot.elements[0];
-             }
+                double m12 = rot.elements[1];
-             else {
+                double m13 = rot.elements[2];
-                 vct.set(-eul.x, -eul.y, eul.z);
+                double m21 = rot.elements[3];
-             }
+                double m31 = rot.elements[6];
-             rotQuat[j].setEulerYZX(vct);
+                double m32 = rot.elements[7];
-         }
+                double m33 = rot.elements[8];
-     }
+                Vector<double> eul = RotMatrixElements2EulerXYZ(m11, m12, m13, m21, m31, m32, m33);
-     XnVector3D  pos = jointPositionData((XnSkeletonJoint)3);
+                if (j==2 || j==3 || j==6 || j==12 || j==17 || j==21) {
-     Vector<double> vect(-pos.Z, pos.X, pos.Y);
+                    if (Xabs(-eul.y-thz)>PI_DIV2) eul.y = -thz;
-     if (!isCalibrated) setStartPosData(vect);
+                }
+                Vector<double> vct(-eul.x, -eul.y, eul.z);    // Mirror: vct(-eul.x, eul.y, -eul.z);
+                rotQuat[j].setEulerYZX(vct);
+                prvQuat[j] = rotQuat[j];
+            }
+            else {
+                rotQuat[j] = prvQuat[j];
+            }
+        }
+    }
-     //
-     //rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];
-     rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
-     rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];
+    // Leg
-     rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
+    rotQuat[22] = ~rotQuat[21]*rotQuat[22];
+    rotQuat[18] = ~rotQuat[17]*rotQuat[18];
-     rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];
+    // Right Hand
-     rotQuat[13] = ~rotQuat[12]*rotQuat[13];
+    rotQuat[13] = ~rotQuat[12]*rotQuat[13];
+    rotQuat[12] = ~rotQuat[ 2]*rotQuat[12];
-     //rotQuat[17] = ~rotQuat[ 3]*rotQuat[17];
+    // Left Hand
-     rotQuat[18] = ~rotQuat[17]*rotQuat[18];
+    rotQuat[ 7] = ~rotQuat[ 6]*rotQuat[ 7];
+    rotQuat[ 6] = ~rotQuat[ 2]*rotQuat[ 6];
-     //rotQuat[21] = ~rotQuat[ 3]*rotQuat[21];
+    rotQuat[ 2] = ~rotQuat[ 3]*rotQuat[ 2];
-     rotQuat[22] = ~rotQuat[21]*rotQuat[22];
+    rotQuat[ 3] = ~rotQuat[ 0]*rotQuat[ 3];
-     // Set to Shared Memory
+    // Set to Shared Memory
-     for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
+    for (int j=1; j<OPENNI_MAX_JOINT_NUM; j++) {
-         int n = OpenNI2SLJointNum[j];
+        int n = OpenNI2SLJointNum[j];
-         if (n>=0) {
+        if (n>=0 && normal_joint[j]) {
-             double* shm = theApp.ptrShm[n];
+            double* shm = theApp.ptrShm[n];
-             if (shm!=NULL) {
+            if (shm!=NULL) {
-                 shm[4] = rotQuat[j].x;
+                shm[4] = rotQuat[j].x;
-                 shm[5] = rotQuat[j].y;
+                shm[5] = rotQuat[j].y;
-                 shm[6] = rotQuat[j].z;
+                shm[6] = rotQuat[j].z;
-                 shm[7] = rotQuat[j].s;
+                shm[7] = rotQuat[j].s;
-             }
+            }
-         }
+        }
-     }
+    }
-     // Position Data from XN_SKEL_TORSO(3) to mPelvis(0)
+    // mPelvis(0)
-     double* shm = theApp.ptrShm[0];
+    double* shm = theApp.ptrShm[0];
-     if (shm!=NULL) {
+    if (shm!=NULL) {
-         shm[1] = (vect.x - startPos.x)/1000.;
+        shm[1] = (posVect[0].x - startPos.x)/1000.;
-         shm[2] = (vect.y - startPos.y)/1000.;
+        shm[2] = (posVect[0].y - startPos.y)/1000.;
-         shm[3] = (vect.z - startPos.z)/1000.;
+        shm[3] = (posVect[0].z - startPos.z)/1000.;
-         shm[4] = rotQuat[0].x;
+        shm[4] = rotQuat[0].x;
-         shm[5] = rotQuat[0].y;
+        shm[5] = rotQuat[0].y;
-         shm[6] = rotQuat[0].z;
+        shm[6] = rotQuat[0].z;
-         shm[7] = rotQuat[0].s;
+        shm[7] = rotQuat[0].s;
-     }
+    }
  }