之前已經在《NiTE2 的人體骨架追蹤》，提過怎麼在 OpenNI 2 的環境下，使用 NiTE2 這個函式庫，來做人體骨架的追蹤了。當時 Heresy 只有提到怎麼去處理骨架的關節點位置資料，而跳過了他的方向性資訊；而 NiTE 2 採用的 Quaternion 表示法似乎對不少人造成問題，所以雖然 NTE 已經不會再維護、發布了，但是這邊還是稍微解釋一下吧…
首先，「Quaternion」（翻譯似乎是「四元數」？）是一種在數學上、可以用來表示三度空間中的旋轉、或是方向性的表示法；他和 euler angles、或是旋轉矩陣這類比較有名的表示法相比，他有一些在實務上的優點，也因此在不少領域上都有用到。（參考：Quaternions and spatial rotation）
而 Heresy 這邊主要是以實用為出發點，不去講 Quaternion 的數學細節；如果要比較學術性、完整的資料，就麻煩自己找書看吧。（其實是因為雖然學過，但是其實也算忘得差不多了 :p）
也希望下面的東西沒有錯了。（有錯也請麻煩指正）
首先，先大概回顧一下 NiTE 的部分：
在 NiTE 2 的 UserTracker 的 UserTrackerFrameRef 中，有提供 getUsers() 這個函式，可以取得目前的所有使用者的資料陣列；而個別的 UserData 則有提供 getSkeleton()、用以取得該使用者的骨架資料、Skeleton。
Skeleton 這個類別紀錄了使用者的人體骨架的十五個關節點的相關資訊，可以用 getJoint() 這個函式來取得單一關節點的個別資料、其型別為 SkeletonJoint；在裡面，除了可以透過 getPosition() 來取得位置資訊外，也可以透過 getOrientation() 來取得關節的方向性，而取得的資料型別就是 Quaternion。
Quaternion 的轉換
Quaternion 的組成基本上是四個數值，包含了 [ w, x, y, z ]。當他用來代表旋轉的時候，意義大致上可以這樣看：
在三度空間裡面，對 ( vx, vy, vz ) 旋轉 θ 度後，Quaternion 的表示形式可以寫成：
[ cos( θ/2 ), sin( θ/2 ) * vx, sin( θ/2 ) * vy, sin( θ/2 ) * vz ]
所以如果要反推算出 ( vx, vy, vz ) 和 θ 的話，程式就是：
const nite::SkeletonJoint& rJoint = rSkeleton.getJoint(nite::JOINT_TORSO);
const nite::Quaternion& tr = rJoint.getOrientation();
double dHalfAngle = acos( tr.w );
double dSinHA = sin( dHalfAngle );
double vx = tr.x / dSinHA,
        vy = tr.y / dSinHA,
        vz = tr.z / dSinHA;
其中，θ 就是 dHalfAngle * 2。透過這樣的轉換，就可以知道 Quaternion 代表的旋轉軸、以及旋轉角了。
而如果要直接轉換成 3×3 旋轉矩陣形式的話，換算方法則是先將 Quaternion normalize：
const float n = 1.0f/sqrt(x*x y*y z*z w*w);
x *= n;
y *= n;
z *= n;
w *= n;
然後再用下面的公式計算出矩陣：
1-2*y*y-2*z*z,   2*x*y-2*z*w,   2*x*z 2*y*w,
  2*x*y 2*z*w, 1-2*x*x-2*z*z,   2*y*z-2*x*w,
  2*x*z-2*y*w,   2*y*z 2*x*w, 1-2*x*x-2*y*y
參考資料：《Convert Quaternion rotation to rotation matrix?》
而沒弄錯的話，NiTE 2 所提供的 Quaternion 基本上都是 normalized 的，所以應該是可以直接用來計算矩陣。如果寫成一個函式的話，基本上會像下面這樣子：
std::array<double,9> q2m( double w, double x, double y, double z )
{
std::array<double,9> matrix = {
  1-2*y*y-2*z*z,   2*x*y-2*z*w,   2*x*z 2*y*w,
   2*x*y 2*z*w, 1-2*x*x-2*z*z,   2*y*z-2*x*w,
   2*x*z-2*y*w,   2*y*z 2*x*w, 1-2*x*x-2*y*y
};
return matrix;
}
而實際上如果是要寫程式處理的話，其實有不少函式庫，都有直接提供 Quaternion 處理功能，而不需要自己重寫一遍。（個人比較納悶的是，OpenCV 居然沒有？）
Qt
像是在 Qt 這個函式庫裡面，也可以使用 QQuaternion（官方文件）來做處理。他的用法大致上是如下：
QQuaternion qTRotation( tr.w, tr.x, tr.y, tr.z );
QVector3D vDir = qTRotation.rotatedVector( QVector3D( 0, 0, -1 ) );
在上面的程式碼裡面，就是先建立出一個 qTRotation 的 Quaternion 物件，然後再去計算 ( 0, 0, -1 ) 這個向量經過這個 Quaternion 旋轉後的結果。
而如果想要轉換成 4×4 的矩陣的話，也可以這樣做：
QMatrix4x4 m;
m.setToIdentity();
m.rotate( qTRotation );
如果進一步，是希望把關節點的位置，轉到 torso 的座標系統的話（附註 1），整個矩陣的計算會是：
const nite::SkeletonJoint& rJoint = rSkeleton.getJoint(nite::JOINT_TORSO);
const nite::Point3f& tt = rJoint.getPosition();
const nite::Quaternion& tr = rJoint.getOrientation();
QQuaternion qTRotation( tr.w, tr.x, tr.y, tr.z );
QMatrix4x4 qMatrix;
qMatrix.setToIdentity();
qMatrix.translate( tt.x, tt.y, tt.z );
qMatrix.rotate( qTRotation );
qMatrix = qMatrix.inverted();
之後，只要把其他的關節點的位置套用這個矩陣，就可以把該點的位置，轉換到以 torso 為中心的座標系統了～下面就是簡單的範例：
const nite::SkeletonJoint& rJointA = rSkeleton.getJoint(nite::JOINT_HEAD);
const nite::Point3f& rPos = rJointA.getPosition();
QVector4D vPos = qMatrix * QVector4D( rPos.x, rPos.y, rPos.z, 1 );
Eigen
而像是 Eigen 這個線性代數的函式庫（官網），也有提供 Quaternion<> 這個類別可以拿來使用（官方文件）；下面是簡單的使用範例：
?Eigen::Quaterniond eTRotation( tr.w, tr.x, tr.y, tr.z );
Eigen::Vector3d vZ = eTRotation * Eigen::Vector3d( 0, 0, -1 );
如果想要和上面 Qt 的例子一樣，把其他關節點的位置轉換到 torso 的座標系統的話，則可以寫成（附註 2）：
const nite::SkeletonJoint& rJoint = rSkeleton.getJoint(nite::JOINT_TORSO);
const nite::Point3f& tt = rJoint.getPosition();
const nite::Quaternion& tr = rJoint.getOrientation();
Eigen::Quaterniond eRotation( tr.w, tr.x, tr.y, tr.z );
Eigen::Translation3d eTRanslate( tt.x, tt.y, tt.z );
Eigen::Affine3d mTransform = eRotation.inverse() * eTRanslate.inverse();
之後要套用到關節點上，則是：
const nite::SkeletonJoint& rJointA = rSkeleton.getJoint(nite::JOINT_HEAD);
const nite::Point3f& rPos = rJointA.getPosition();
Eigen::Vector3d ePos = mTransform * Eigen::Vector3d( rPos.x, rPos.y, rPos.z );
附註：
-
這樣做的好處，是可以在某些情況下減少判斷的複雜度。比如果要判斷「手是否往前伸的夠遠」，在本來的座標系統上，需要先判斷目前人是面向哪一面、手的方向和人的方向是否一致；但是如果把全部的關節點的位置都轉換到軀幹的座標系統上的話，那只要判斷手部位置的 Z 的值是否夠小（往前伸會是負的），就可以了。
-
這邊 Heresy 是先把 eTranslate 和 eRotation 先做 inverse 再相乘、求出 affine transform matrix、也就是 mTransform；不過也可以先相乘後再做 inverse：
Eigen::Affine3d mTransform = ( eTRanslate * eRotation ).inverse();Heresy 不確定哪個效率好就是了。