add test that demonstrated bug in tf::Matrix3x3

tf::Matrix3x3::getEulerYPR() has a bug which returns an incorrect rpy
for certain corner case inputs.  This test demonstrates that bug.
fix bug in tf::Matrix3x3::getEulerYPR()

Fixes a bug in tf::Matrix3x3::getEulerYPR() implementation's handling
of gimbal lock cases (when the new x axis aligns with the old +/-z
axis).