def testQuaternionMatrix(self): q = matmath.axisAngleToQuaternion([1, 0, 0], np.radians(90)) qmat = matmath.quaternionToRotationMatrix(q) rmat = matmath.xRotationMatrix(math.radians(90)) np.testing.assert_almost_equal(qmat, rmat) q = matmath.axisAngleToQuaternion([0, 1, 0], np.radians(90)) qmat = matmath.quaternionToRotationMatrix(q) rmat = matmath.yRotationMatrix(math.radians(90)) np.testing.assert_almost_equal(qmat, rmat) q = matmath.axisAngleToQuaternion([0, 0, 1], np.radians(90)) qmat = matmath.quaternionToRotationMatrix(q) rmat = matmath.zRotationMatrix(math.radians(90)) np.testing.assert_almost_equal(qmat, rmat)
def test2M(self): # 2 Meters away depth scan pt = np.array([0, 0, 2, 1]) print "PC", pt mat = matmath.pcToSoSMatrix() npt = pt.dot(mat) print "SoS ", npt trans = np.array([0, 0, 0]) quaternion = matmath.axisAngleToQuaternion([1, 0, 0], np.radians(90)) mat = matmath.getPC2WorldMatrix(trans, quaternion) npt = pt.dot(mat) print "Device", npt pt = np.array([0, 1, 2, 1]) print "PC", pt mat = matmath.pcToSoSMatrix() npt = pt.dot(mat) print "SoS ", npt mat = matmath.getPC2WorldMatrix(trans, quaternion) npt = pt.dot(mat) print "Device", npt