diff --git a/.github/workflows/master.yml b/.github/workflows/master.yml index 9e8a7fa7..210d1237 100644 --- a/.github/workflows/master.yml +++ b/.github/workflows/master.yml @@ -77,6 +77,12 @@ jobs: file: ./coverage.xml sphinx: # If the above worked: - # Build docs and upload to GH Pages + # Build docs and upload to GH Pages -- only on push (merge to master/future), + # never on pull_request: a PR whose branch lives in a fork (including the + # author's own fork) gets a read-only GITHUB_TOKEN by GitHub's own design, + # so the gh-pages push step always fails there regardless of repo settings. + # Publishing unreviewed PR content to the live docs site isn't desired + # either way. needs: unittest + if: ${{ github.event_name != 'pull_request' }} uses: ./.github/workflows/sphinx.yml diff --git a/.github/workflows/sphinx.yml b/.github/workflows/sphinx.yml index c0beb95a..445d7181 100644 --- a/.github/workflows/sphinx.yml +++ b/.github/workflows/sphinx.yml @@ -6,7 +6,6 @@ on: jobs: sphinx: runs-on: ubuntu-22.04 - # if: ${{ github.event_name != 'pull_request' }} steps: - uses: actions/checkout@v4 - name: Set up Python 3.12 diff --git a/pyproject.toml b/pyproject.toml index 10b74a4c..1597ad25 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -37,7 +37,7 @@ dependencies = [ "scipy", "matplotlib", "ansitable", - "typing_extensions", + "typing_extensions>=4.0.0", "pre-commit", ] diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py index e9bc0e70..b33590ae 100644 --- a/spatialmath/base/transforms2d.py +++ b/spatialmath/base/transforms2d.py @@ -1312,7 +1312,11 @@ def _FindCorrespondences( num_iter += 1 - return T + # T accumulates roundoff error through repeated composition + # (T = T @ new_T) over the iterations above, and can drift just + # outside ishom2()'s orthonormality tolerance -- restore exact + # orthonormality before returning. + return trnorm2(T) if _matplotlib_exists: diff --git a/spatialmath/base/vectors.py b/spatialmath/base/vectors.py index b0055e9a..59c61797 100644 --- a/spatialmath/base/vectors.py +++ b/spatialmath/base/vectors.py @@ -567,13 +567,10 @@ def wrap_mpi2_pi2(theta: ArrayLike) -> Union[float, NDArray]: """ theta = getvector(theta) - n = theta / np.pi * 2 - if isinstance(n, np.ndarray): - n = n.astype(int) - else: - n = np.fix(n).astype(int) - - y = np.where(np.bitwise_and(n, 1) == 0, theta - n * np.pi, n * np.pi - theta) + theta = np.fmod(theta, 2 * np.pi) + theta = np.where(theta > np.pi, theta - 2 * np.pi, theta) + theta = np.where(theta < -np.pi, theta + 2 * np.pi, theta) + y = np.where(np.abs(theta) <= np.pi / 2, theta, np.copysign(np.pi, theta) - theta) if isinstance(y, np.ndarray) and len(y) == 1: return float(y[0]) else: @@ -832,7 +829,14 @@ def project(v1: ArrayLike3, v2: ArrayLike3) -> ArrayLike3: :return: vector projection of v1 onto v2 (parrallel to v2) :rtype: ndarray(n) """ - return np.dot(v1, v2) * v2 + v1 = getvector(v1) + v2 = getvector(v2) + scale = np.max(np.abs(v2)) + if scale == 0: + return np.zeros_like(v2) + # Normalize the scale before dot products to avoid over/underflow. + v2 = v2 / scale + return (np.dot(v1, v2) / np.dot(v2, v2)) * v2 def orthogonalize(v1: ArrayLike3, v2: ArrayLike3, normalize: bool = True) -> ArrayLike3: diff --git a/spatialmath/geom3d.py b/spatialmath/geom3d.py index edc4dc08..17f54a76 100755 --- a/spatialmath/geom3d.py +++ b/spatialmath/geom3d.py @@ -836,7 +836,7 @@ def distance( :param l2: Second line :type l2: ``Line3`` - :param tol: Tolerance in multiples of eps, defaults to 20 + :param tol: Parallel-direction tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: Closest distance between lines :rtype: float @@ -847,20 +847,16 @@ def distance( :seealso: :meth:`closest_to_line` """ - if l1 | l2: - # lines are parallel - l = np.cross( - l1.w, l1.v - l2.v * np.dot(l1.w, l2.w) / dot(l2.w, l2.w) - ) / np.linalg.norm(l1.w) - else: - # lines are not parallel - if abs(l1 * l2) < tol * _eps: - # lines intersect at a point - l = 0 - else: - # lines don't intersect, find closest distance - l = abs(l1 * l2) / np.linalg.norm(np.cross(l1.w, l2.w)) ** 2 - return l + w1, w2 = l1.uw, l2.uw + normal = np.cross(w1, w2) + normal_length = np.linalg.norm(normal) + delta = l2.pp - l1.pp + if normal_length <= tol * _eps: + # Parallel lines: remove the component along the common direction. + return float(np.linalg.norm(np.cross(delta, w1))) + # The displacement along the common normal is the shortest distance. + # Unit directions make the result independent of Plucker scaling. + return float(abs(np.dot(delta, normal)) / normal_length) def closest_to_line( l1, l2: Line3 # type:ignore diff --git a/spatialmath/quaternion.py b/spatialmath/quaternion.py index a5759709..6fad5597 100644 --- a/spatialmath/quaternion.py +++ b/spatialmath/quaternion.py @@ -381,6 +381,9 @@ def log(self) -> Quaternion: \ln \| q \|, \langle \frac{\vec{v}}{\| \vec{v} \|} \cos^{-1} \frac{s}{\| q \|} \rangle + The implementation uses the equivalent :math:`\operatorname{atan2}(\|\vec{v}\|, s)` + to avoid the loss of precision in ``acos`` for small rotation angles. + For a ``UnitQuaternion`` the logarithm is a pure quaternion whose vector part :math:`\vec{v}` and :math:`\vec{v}/2` is a Euler vector: parallel to the axis of rotation and whose norm is the magnitude of rotation. @@ -403,20 +406,17 @@ def log(self) -> Quaternion: norm = self.norm() s = np.log(norm) if len(self) == 1: - if smb.iszerovec(self._A[1:4]): + n = math.hypot(*self._A[1:4]) + if n == 0: v = np.zeros((3,)) else: - v = math.acos(np.clip(self._A[0] / norm, -1, 1)) * smb.unitvec( - self._A[1:4] - ) + v = math.atan2(n, self._A[0]) * self._A[1:4] / n return Quaternion(s=s, v=v) else: - v = [ - np.zeros((3,)) - if smb.iszerovec(A[1:4]) - else math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4]) - for A, n in zip(self._A, norm) - ] + v = [] + for A in self._A: + n = math.hypot(*A[1:4]) + v.append(np.zeros((3,)) if n == 0 else math.atan2(n, A[0]) * A[1:4] / n) return Quaternion(s=s, v=np.array(v)) def exp(self, tol: float = 20) -> Quaternion: diff --git a/tests/base/test_transforms2d.py b/tests/base/test_transforms2d.py index 6a406006..bf578b22 100755 --- a/tests/base/test_transforms2d.py +++ b/tests/base/test_transforms2d.py @@ -174,6 +174,22 @@ def test_icp2d(self): T2 = ICP2d(p2, p1, T=xyt2tr([1, 2, 0.2])) nt.assert_almost_equal(T, T2) + def test_icp2d_returns_valid_se2(self): + # noisy correspondences plus a forced iteration count accumulate + # roundoff error in the T = T @ new_T composition -- the result + # must still pass ishom2(check=True), ie. be exactly orthonormal. + # seed fixed: this configuration reliably triggers the drift. + np.random.seed(0) + p1 = np.random.uniform(size=(2, 30)) + T = xyt2tr([1, 2, 0.2]) + + p2 = homtrans(T, p1) + np.random.normal(scale=0.01, size=(2, 30)) + k = np.random.permutation(p2.shape[1]) + p2 = p2[:, k] + + T2 = ICP2d(p2, p1, max_iter=100, min_delta_err=0) + self.assertTrue(ishom2(T2, check=True)) + def test_print2(self): T = transl2(1, 2) @ trot2(0.3) diff --git a/tests/base/test_vectors.py b/tests/base/test_vectors.py index 15c6a451..3110eb56 100755 --- a/tests/base/test_vectors.py +++ b/tests/base/test_vectors.py @@ -33,6 +33,23 @@ class TestVector(unittest.TestCase): def tearDownClass(cls): plt.close("all") + def test_project_nonunit_direction(self): + for scale in (1, 2, -3, 1e-200, 1e200): + direction = np.array([scale, scale, 0]) + nt.assert_allclose(project([3, 1, 4], direction), [2, 2, 0]) + nt.assert_allclose(project((3, 1, 4), (2, 2, 0)), [2, 2, 0]) + + def test_orthogonalize_nonunit_direction(self): + result = orthogonalize([3, 1, 4], [2, 2, 0], normalize=False) + nt.assert_allclose(result, [1, -1, 4]) + self.assertAlmostEqual(np.dot(result, [2, 2, 0]), 0) + normalized = orthogonalize([3, 1, 4], [2, 2, 0]) + self.assertAlmostEqual(np.linalg.norm(normalized), 1) + self.assertAlmostEqual(np.dot(normalized, [2, 2, 0]), 0) + + def test_project_zero_direction(self): + nt.assert_array_equal(project([1, 2, 3], [0, 0, 0]), [0, 0, 0]) + def test_unit(self): nt.assert_array_almost_equal(unitvec([1, 0, 0]), np.r_[1, 0, 0]) nt.assert_array_almost_equal(unitvec([0, 1, 0]), np.r_[0, 1, 0]) @@ -278,6 +295,30 @@ def test_angdiff(self): nt.assert_array_almost_equal(x, [0, 0, 0]) self.assertIsInstance(x, np.ndarray) + def test_latitude_wrap_multiple_turns(self): + angles = pi * np.arange(-4, 4.5, 0.5) + expected = pi * np.array( + [0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0, 0.5, 0, -0.5, 0] + ) + nt.assert_allclose(wrap_mpi2_pi2(angles), expected, atol=1e-14) + nt.assert_allclose(angle_wrap(angles, mode="-pi/2:pi/2"), expected, atol=1e-14) + for angle, folded in zip(angles, expected): + self.assertAlmostEqual(wrap_mpi2_pi2(angle), folded) + + def test_latitude_wrap_boundaries_and_small_angles(self): + epsilon = 1e-12 + angles = np.array( + [-pi / 2 - epsilon, -pi / 2 + epsilon, pi / 2 - epsilon, pi / 2 + epsilon] + ) + expected = np.array( + [-pi / 2 + epsilon, -pi / 2 + epsilon, pi / 2 - epsilon, pi / 2 - epsilon] + ) + nt.assert_allclose(wrap_mpi2_pi2(angles), expected, rtol=0, atol=1e-15) + nt.assert_allclose( + wrap_mpi2_pi2([1e-16, -1e-16]), [1e-16, -1e-16], rtol=0, atol=0 + ) + self.assertEqual(wrap_mpi2_pi2([]).size, 0) + def test_wrap(self): self.assertAlmostEqual(wrap_0_2pi(0), 0) self.assertAlmostEqual(wrap_0_2pi(2 * pi), 0) diff --git a/tests/test_geom3d.py b/tests/test_geom3d.py index 81a50fdb..bc6f2e63 100755 --- a/tests/test_geom3d.py +++ b/tests/test_geom3d.py @@ -17,6 +17,36 @@ import matplotlib.pyplot as plt +@pytest.mark.parametrize( + "p1, w1, p2, w2, expected", + [ + ([0, 0, 0], [1, 0, 0], [0, 3, 4], [1, 0, 0], 5), + ([1, 2, 3], [1, 0, 0], [4, 2, 3], [-1, 0, 0], 0), + ([1, 2, 3], [2, 0, 0], [1, 2, 3], [0, 5, 0], 0), + ([0, 0, 0], [1, 0, 0], [0, 0, 2], [0.6, 0.8, 0], 2), + ([1, 2, 3], [2, 0, 0], [1, 2, 5], [3, 4, 0], 2), + ], + ids=["parallel", "coincident", "intersecting", "skew-unit", "skew-scaled"], +) +@pytest.mark.parametrize("scale1, scale2", [(1, 1), (2, 5), (-2, 0.25), (1e-8, 1e-8)]) +def test_line_distance(p1, w1, p2, w2, expected, scale1, scale2): + # Scaling either set of Plucker coordinates does not change the line. + line1 = Line3.PointDir(p1, np.array(w1) * scale1) + line2 = Line3.PointDir(p2, np.array(w2) * scale2) + for first, second in [(line1, line2), (line2, line1)]: + distance = first.distance(second) + assert np.isscalar(distance) + assert distance == pytest.approx(expected, abs=1e-12) + + +def test_line_distance_parallel_tolerance(): + line1 = Line3.PointDir([0, 0, 0], [1, 0, 0]) + line2 = Line3.PointDir([0, 1, 2], [1, 1e-8, 0]) + assert line1.distance(line2) == pytest.approx(2) + # A larger angular tolerance treats the directions as parallel. + assert line1.distance(line2, tol=1e8) == pytest.approx(np.sqrt(5)) + + class Line3Test(unittest.TestCase): # Primitives def test_constructor1(self): diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py index 8a87a433..683fd315 100644 --- a/tests/test_quaternion.py +++ b/tests/test_quaternion.py @@ -943,6 +943,60 @@ def test_log(self): self.assertIsInstance(qq, UnitQuaternion) nt.assert_array_almost_equal(qq.vec, np.r_[1, 0, 0, 0]) + def test_log_small_rotation(self): + axis = np.array([1.0, -2.0, 3.0]) / math.sqrt(14) + for angle in (1e-12, -1e-9, 1e-7, 1e-4, 0.3): + for scale in (1.0, 3.0, 1e-6): + with self.subTest(angle=angle, scale=scale): + values = scale * np.r_[cos(angle / 2), sin(angle / 2) * axis] + q = UnitQuaternion(values) if scale == 1 else Quaternion(values) + original = q.vec.copy() + qlog = q.log() + nt.assert_allclose(qlog.v, angle / 2 * axis, rtol=1e-13, atol=0) + self.assertAlmostEqual(qlog.s, math.log(scale)) + nt.assert_allclose(qlog.exp().vec, q.vec, rtol=1e-13, atol=0) + nt.assert_array_equal(q.vec, original) + + def test_log_small_rotation_sequence(self): + angles = np.array([1e-12, -1e-9, 1e-7, 0.3]) + axes = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 0, 0]]) + values = np.column_stack( + (np.cos(angles / 2), np.sin(angles[:, None] / 2) * axes) + ) + expected = angles[:, None] / 2 * axes + for scales in ( + np.ones(4), + np.array([2.0, 3.0, 4.0, 5.0]), + 1e-6 * np.ones(4), + ): + with self.subTest(scales=scales): + scaled = scales[:, None] * values + q = ( + UnitQuaternion(scaled) + if np.all(scales == 1) + else Quaternion(list(scaled)) + ) + original = q.vec.copy() + qlog = q.log() + self.assertEqual(len(qlog), len(q)) + nt.assert_allclose(qlog.v, expected, rtol=1e-13, atol=0) + nt.assert_allclose(qlog.s, np.log(scales), atol=1e-15) + for i in range(len(q)): + nt.assert_allclose(qlog[i].exp().vec, scaled[i], rtol=1e-13, atol=0) + nt.assert_allclose(qlog[i].vec, q[i].log().vec, rtol=1e-13, atol=0) + nt.assert_array_equal(q.vec, original) + + def test_log_near_negative_real(self): + # Keep the principal quaternion branch near pi, rather than folding + # a negative scalar part onto the small-angle branch. + angle = 1e-7 + q = Quaternion([-cos(angle), sin(angle), 0, 0]) + expected = np.r_[0, pi - angle, 0, 0] + nt.assert_allclose(q.log().vec, expected, rtol=0, atol=1e-15) + nt.assert_allclose( + Quaternion([q, q]).log().vec, [expected, expected], rtol=0, atol=1e-15 + ) + def test_concat(self): u = Quaternion() uu = Quaternion([u, u, u, u])