處理 NiTE2 的骨架關節點方向性資訊:Quaternion

| | 0 Comments| 16:06
Categories:

之前已經在《NiTE2 的人體骨架追蹤》,提過怎麼在 OpenNI 2 的環境下,使用 NiTE2 這個函式庫,來做人體骨架的追蹤了。當時 Heresy 只有提到怎麼去處理骨架的關節點位置資料,而跳過了他的方向性資訊;而 NiTE 2 採用的 Quaternion 表示法似乎對不少人造成問題,所以雖然 NTE 已經不會再維護、發布了,但是這邊還是稍微解釋一下吧…

首先,「Quaternion」(翻譯似乎是「四元數」?)是一種在數學上、可以用來表示三度空間中的旋轉、或是方向性的表示法;他和 euler angles、或是旋轉矩陣這類比較有名的表示法相比,他有一些在實務上的優點,也因此在不少領域上都有用到。(參考:Quaternions and spatial rotation

而 Heresy 這邊主要是以實用為出發點,不去講 Quaternion 的數學細節;如果要比較學術性、完整的資料,就麻煩自己找書看吧。(其實是因為雖然學過,但是其實也算忘得差不多了 :p)
也希望下面的東西沒有錯了。(有錯也請麻煩指正)

首先,先大概回顧一下 NiTE 的部分:

在 NiTE 2 的 UserTrackerUserTrackerFrameRef 中,有提供 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 );

附註:

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

  2. 這邊 Heresy 是先把 eTranslateeRotation 先做 inverse 再相乘、求出 affine transform matrix、也就是 mTransform;不過也可以先相乘後再做 inverse:

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

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

Leave a Reply

發佈留言必須填寫的電子郵件地址不會公開。 必填欄位標示為 *