讓智慧手機變成深度感應器！LazeeEye

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

像是在 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 這個線性代數的函式庫（官網），也有提供 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 );

1. 這樣做的好處，是可以在某些情況下減少判斷的複雜度。比如果要判斷「手是否往前伸的夠遠」，在本來的座標系統上，需要先判斷目前人是面向哪一面、手的方向和人的方向是否一致；但是如果把全部的關節點的位置都轉換到軀幹的座標系統上的話，那只要判斷手部位置的 Z 的值是否夠小（往前伸會是負的），就可以了。

2. 這邊 Heresy 是先把 eTranslate 和 eRotation 先做 inverse 再相乘、求出 affine transform matrix、也就是 mTransform；不過也可以先相乘後再做 inverse：

   Eigen::Affine3d mTransform = ( eTRanslate * eRotation ).inverse();

   Heresy 不確定哪個效率好就是了。