From c14ef3048726d9d15e88003f3548602022128840 Mon Sep 17 00:00:00 2001
From: Matthew Brett <matthew.brett@gmail.com>
Date: Fri, 21 Aug 2015 23:21:57 +0100
Subject: [PATCH] ENH: allow threshold for mat2axangle precision

Allow user to set precision for mat2axangle check for rotation matrix
precision.

Make default threshold more liberal.
---
 transforms3d/axangles.py            |  8 ++++++--
 transforms3d/tests/test_axangles.py | 20 +++++++++++++++++++-
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/transforms3d/axangles.py b/transforms3d/axangles.py
index 4e7a364..4d83e1c 100644
--- a/transforms3d/axangles.py
+++ b/transforms3d/axangles.py
@@ -112,12 +112,16 @@ def axangle2aff(axis, angle, point=None):
     return M
 
 
-def mat2axangle(mat):
+def mat2axangle(mat, unit_thresh=1e-5):
     """Return axis, angle and point from (3, 3) matrix `mat`
 
     Parameters
     ----------
     mat : array-like shape (3, 3)
+        Rotation matrix
+    unit_thresh : float, optional
+        Tolerable difference from 1 when testing for unit eigenvalues to
+        confirm `mat` is a rotation matrix.
 
     Returns
     -------
@@ -143,7 +147,7 @@ def mat2axangle(mat):
     M = np.asarray(mat, dtype=np.float)
     # direction: unit eigenvector of R33 corresponding to eigenvalue of 1
     L, W = np.linalg.eig(M.T)
-    i = np.where(abs(np.real(L) - 1.0) < 1e-8)[0]
+    i = np.where(np.abs(L - 1.0) < unit_thresh)[0]
     if not len(i):
         raise ValueError("no unit eigenvector corresponding to eigenvalue 1")
     direction = np.real(W[:, i[-1]]).squeeze()
diff --git a/transforms3d/tests/test_axangles.py b/transforms3d/tests/test_axangles.py
index 0ab4010..d60efc9 100644
--- a/transforms3d/tests/test_axangles.py
+++ b/transforms3d/tests/test_axangles.py
@@ -9,7 +9,7 @@
 
 from .samples import euler_tuples
 
-from numpy.testing import assert_array_almost_equal
+from numpy.testing import assert_array_almost_equal, assert_almost_equal
 
 from nose.tools import assert_raises
 
@@ -42,6 +42,24 @@ def test_aa_points():
                 assert_array_almost_equal(RP, RP_back)
 
 
+def test_mat2axangle_thresh():
+    # Test precision threshold to mat2axangle
+    axis, angle = mat2axangle(np.eye(3))
+    assert_almost_equal(axis, [0, 0, 1])
+    assert_almost_equal(angle, 0)
+    offset = 1e-6
+    mat = np.diag([1 + offset] * 3)
+    axis, angle = mat2axangle(mat)
+    assert_almost_equal(axis, [0, 0, 1])
+    assert_almost_equal(angle, 0)
+    offset = 1e-4
+    mat = np.diag([1 + offset] * 3)
+    assert_raises(ValueError, mat2axangle, mat)
+    axis, angle = mat2axangle(mat, 1e-4)
+    assert_almost_equal(axis, [0, 0, 1])
+    assert_almost_equal(angle, 0)
+
+
 def test_angle_axis_imps():
     for x, y, z in euler_tuples:
         M = ttb.euler2mat(z, y, x)