Log in or register to post comments

Rotation around Z-axis from pose matrix

January 7, 2012 - 6:11am #1

I'd like to retrieve the rotation around the z-axis that is applied to a trackable to compare it to the rotation around the z-axis applied to another trackable. So I essentially need the differente in rotation so that I know how much more the second trackable is rotated relative to the other one.

I can get the pose matrix of a trackable, but I don't know how to extract the rotation around the z-axis now.

Any help would be appreciated.

Re: Rotation around Z-axis from pose matrix

January 8, 2012 - 7:45pm #3

Thanks SteveRogers :)

It worked like a charm (after finding another very small Quaternion class that was compatible with Linux and C++ out of the box).

Re: Rotation around Z-axis from pose matrix

January 8, 2012 - 4:16am #2

Hi Wesley, I don't know how helpful this is - it works for me but it's very grubby. In case someone's following me Arne Saknussemm-style I hope it's of some use.

(I work in quaternions so this includes that step. I use Will Perone's [I" />awesome[/I" /> public domain math library at http://willperone.net/Code/Math.zip. It would be much better to do the "weird" rotation & invert in the middle with a suitable, simple, 3x3 rotation matrix.))

[CODE" />QCAR::Matrix44F mat = QCAR::Tool::convertPose2GLMatrix(trackable->getPose());

// get translation (-y because up/down seems reversed; -z because my axis points away from viewer)

vector3f translation(mat.data[12" />, -mat.data[13" />, -mat.data[14" />);

// make quaternion

float trace = 1 + mat.data[0" /> + mat.data[5" /> + mat.data[10" />;
float t2 = sqrtf(trace) * 2;
float qx = (mat.data[9" /> - mat.data[6" />) / t2;
float qy = (mat.data[2" /> - mat.data[8" />) / t2;
float qz = (mat.data[4" /> - mat.data[1" />) / t2;
float qw = 0.25 * t2;
quaternion qr(qw, qx, qy, qz);

// weird - must rotate 180 degrees around x axis and invert/conjugate for some reason
// I won't detail these - I used Will Perone... one reason it would be better to stay in matrices

qr *= quaternion::from_axis_angle(vector3f(1,0,0), M_PI);
qr.conjugate();

// quaternion to euler angles

//float sqw = qr.w*qr.w; don't need this it seems...
float sqx = qr.x*qr.x;
float sqy = qr.y*qr.y;
float sqz = qr.z*qr.z;

float eulerx = atan2f(2.0 * (qr.z*qr.y + qr.x*qr.w), 1 - 2*(sqx + sqy));
float eulery = asinf(-2.0 * (qr.x*qr.z - qr.y*qr.w));
float eulerz = atan2f(2.0 * (qr.x*qr.y + qr.z*qr.w), 1 - 2*(sqy + sqz));
[/CODE" />

I hope I typed this right. No doubt it could be mucho simplified and perhaps someone out there can post the code to do it.

Log in or register to post comments