diff --git a/spatialmath/base/transformsNd.py b/spatialmath/base/transformsNd.py index 611c89a3..d1af76d9 100644 --- a/spatialmath/base/transformsNd.py +++ b/spatialmath/base/transformsNd.py @@ -709,12 +709,15 @@ def h2e(v: NDArray) -> NDArray: :param v: homogeneous vector or matrix :type v: array_like(n), ndarray(n,m) :return: Euclidean vector - :rtype: ndarray(n-1), ndarray(n-1,m) + :rtype: ndarray(n-1,1), ndarray(n-1,m) + :raises ValueError: if ``v`` has fewer than two rows, or is not a vector or + matrix - - If ``v`` is an N-vector, return an (N-1)-column vector where the elements have - all been scaled by the last element of ``v``. - - If ``v`` is a matrix (NxM), return a matrix (N-1xM), where each column has - been scaled by its last element. + Each *column* of ``v`` is a homogeneous point, and is scaled by its last element: + + - If ``v`` is an N-vector, shape (N,) or (N,1), or a list or tuple, return an + (N-1,1) column vector. + - If ``v`` is a matrix (NxM), return a matrix (N-1xM). .. runblock:: pycon @@ -726,16 +729,24 @@ def h2e(v: NDArray) -> NDArray: >>> h2e(h) .. note:: The result is always a 2D array, a 1D input results in a column vector. + .. note:: A 2D array is always a matrix of column vectors, rows are never + points. To convert a row vector of shape (1,N) pass its transpose. A + homogeneous point needs at least two elements, so an input with a single + row, such as shape (1,N), raises a ``ValueError``. - :seealso: e2h + :seealso: :func:`e2h` """ if isinstance(v, np.ndarray) and len(v.shape) == 2: # dealing with matrix + if v.shape[0] < 2: + raise ValueError("homogeneous input must have at least 2 rows") return v[:-1, :] / v[-1, :][np.newaxis, :] elif isvector(v): # dealing with shape (N,) array v = getvector(v, out="col") + if v.shape[0] < 2: + raise ValueError("homogeneous input must have at least 2 elements") return v[0:-1] / v[-1] else: @@ -749,12 +760,15 @@ def e2h(v: NDArray) -> NDArray: :param v: Euclidean vector or matrix :type v: array_like(n), ndarray(n,m) :return: homogeneous vector - :rtype: ndarray(n+1,m) + :rtype: ndarray(n+1,1), ndarray(n+1,m) + :raises ValueError: if ``v`` is not a vector or matrix + + Each *column* of ``v`` is a Euclidean point, and has a 1 appended: - - If ``v`` is an N-vector, return an (N+1)-column vector where a value of 1 has - been appended as the last element. - - If ``v`` is a matrix (NxM), return a matrix (N+1xM), where each column has - been appended with a value of 1, ie. a row of ones has been appended to the matrix. + - If ``v`` is an N-vector, shape (N,) or (N,1), or a list or tuple, return an + (N+1,1) column vector. + - If ``v`` is a matrix (NxM), return a matrix (N+1xM), ie. a row of ones has + been appended to the matrix. .. runblock:: pycon @@ -765,8 +779,12 @@ def e2h(v: NDArray) -> NDArray: >>> e2h(e) .. note:: The result is always a 2D array, a 1D input results in a column vector. + .. note:: A 2D array is always a matrix of column vectors, rows are never + points. A row vector of shape (1,N) is therefore N one-dimensional + points and the result has shape (2,N). To convert a row vector to a + single point pass its transpose. - :seealso: e2h + :seealso: :func:`h2e` """ if isinstance(v, np.ndarray) and len(v.shape) == 2: # dealing with matrix diff --git a/tests/base/test_transformsNd.py b/tests/base/test_transformsNd.py index 14c7fd42..7291eaa2 100755 --- a/tests/base/test_transformsNd.py +++ b/tests/base/test_transformsNd.py @@ -261,6 +261,64 @@ def test_homog(self): nt.assert_almost_equal(h2e([2, 4, 6, 2]), np.c_[1, 2, 3].T) + def test_e2h_vector(self): + # list, tuple, 1D array, column vector all give an (N+1,1) column + expected = np.c_[1, 2, 3, 1].T + for v in ([1, 2, 3], (1, 2, 3), np.r_[1, 2, 3], np.c_[1, 2, 3].T): + with self.subTest(type=type(v).__name__): + out = e2h(v) + self.assertEqual(out.shape, (4, 1)) + nt.assert_almost_equal(out, expected) + + def test_e2h_matrix(self): + # each column gains a trailing 1 + e = np.c_[[1, 2], [3, 4], [5, 6]] + nt.assert_almost_equal(e2h(e), np.array([[1, 3, 5], [2, 4, 6], [1, 1, 1]])) + + def test_h2e_vector(self): + # the result is scaled by the last element + expected = np.c_[1, 2, 3].T + for v in ([2, 4, 6, 2], (2, 4, 6, 2), np.r_[2, 4, 6, 2], np.c_[2, 4, 6, 2].T): + with self.subTest(type=type(v).__name__): + out = h2e(v) + self.assertEqual(out.shape, (3, 1)) + nt.assert_almost_equal(out, expected) + + def test_h2e_matrix(self): + # each column is scaled by its own last element + h = np.c_[[1, 2, 1], [3, 4, 2], [5, 6, 1]] + nt.assert_almost_equal(h2e(h), np.array([[1, 1.5, 5], [2, 2, 6]])) + + def test_homog_roundtrip(self): + v = np.c_[[1, 2, 3], [-4, 5, 0.5]] + nt.assert_almost_equal(h2e(e2h(v)), v) + + h = np.c_[[2, 4, 6, 2], [3, 6, 9, 3]] + nt.assert_almost_equal(e2h(h2e(h)), np.c_[[1, 2, 3, 1], [1, 2, 3, 1]]) + + def test_homog_row_vector_is_matrix(self): + # Documents (rather than endorses) current behavior: a 2D array is + # always a matrix of column vectors, so a (1,N) row is N 1-vectors. + # Changing this would silently break callers passing 1xN matrices. + self.assertEqual(e2h(np.ones((1, 3))).shape, (2, 3)) + nt.assert_almost_equal(e2h(np.c_[[1, 2, 3]].T), np.array([[1, 2, 3], [1, 1, 1]])) + nt.assert_almost_equal(h2e(np.array([[1.0, 2, 4], [1, 2, 2]])), np.array([[1, 1, 2]])) + + def test_h2e_too_few_rows(self): + # a homogeneous point needs at least 2 elements, h2e of 1 row would be + # an empty (0,N) array + for bad in (np.ones((1, 3)), np.ones((1, 1)), [5], 5, np.ones((1,))): + with self.subTest(value=repr(bad)): + with self.assertRaises(ValueError): + h2e(bad) + + def test_homog_bad_type(self): + for convert in (e2h, h2e): + for bad in ("hello", None, np.empty((0,))): + with self.subTest(convert=convert.__name__, value=repr(bad)): + with self.assertRaises(ValueError): + convert(bad) + def test_homtrans(self): # 3D T = trotx(pi / 2, t=[1, 2, 3])