Repository: scipy/scipy

Issue:
Add LAPACK's ZUNCSD cosine sine decomposition
**Is your feature request related to a problem? Please describe.**

I would like to use Cosine Sine decomposition in certain quantum compilation algorithms to decompose 3 qubit circuits - if this project of mine succeeds this will be implemented in Google's [Cirq](https://github.com/quantumlib/Cirq/issues/451). CSD is Central to the [algorithm](https://arxiv.org/abs/quant-ph/0406176). I do have a prototype working with https://github.com/anuesseler/pycsd however I believe SciPy would be a great place for this function. 

**Describe the solution you'd like**

As scipy already has a mature process of importing and distributing compiled LAPACK functions, I would like to have ZUNCSD exposed through scipy.linalg. (Eventually maybe it would be great to have a higher level implementation of CSD amongst the other matrix decompositions.) I believe other python based quantum frameworks could benefit from having zuncsd exposed this way. 

**Describe alternatives you've considered**

Alternatives considered: 

- [transpile from LAPACK](http://www.netlib.org/lapack/explore-html/de/d0d/zuncsd_8f_source.html) ZUNCSD  with dependencies or [reimplement from the original paper](https://arxiv.org/abs/0707.1838) myself  - this seems very laborious and probably going to end up being significantly slower - maybe I could cut some corners by limiting to 8x8 unitaries. 
- add fortran wrapper dependency inspired by https://github.com/anuesseler/pycsd to Cirq. This will be the first dependency there to introduce fortran compilation within Cirq that would complicate the build system there. I think this would be very hard to push through. 
- fork / pick up https://github.com/anuesseler/pycsd and publish a separate library as a dependency - this is possible but in the end but wouldn't be that much different from Cirq maintaining this code in the end - also it has GPL3 licensing so it won't work verbatim for us. 

**Additional context (e.g. screenshots)**

I am open to raise a PR for this (guidance is welcome if you are open to adding this). 



Discussion:
This sounds nice. We need to add also `sorcsd`, `dorcsd`, and `cunscd `as a group. @mdhaber Is this along with your lapack project? 
@mdhaber, @ilayn, if you're okay to provide feedback I'm happy to give this a try and open a PR
No, this is not on the docket. Go for it! Thanks @balopat!

Resolving patch:
diff --git a/scipy/linalg/__init__.py b/scipy/linalg/__init__.py
--- a/scipy/linalg/__init__.py
+++ b/scipy/linalg/__init__.py
@@ -95,7 +95,8 @@
    rsf2csf - Real to complex Schur form
    hessenberg - Hessenberg form of a matrix
    cdf2rdf - Complex diagonal form to real diagonal block form
-
+   cossin - Cosine sine decomposition of a unitary or orthogonal matrix   
+   
 .. seealso::
 
    `scipy.linalg.interpolative` -- Interpolative matrix decompositions
@@ -212,6 +213,7 @@
 from ._procrustes import *
 from ._decomp_update import *
 from ._sketches import *
+from ._decomp_cossin import *
 
 __all__ = [s for s in dir() if not s.startswith('_')]
 
diff --git a/scipy/linalg/_decomp_cossin.py b/scipy/linalg/_decomp_cossin.py
new file mode 100644
--- /dev/null
+++ b/scipy/linalg/_decomp_cossin.py
@@ -0,0 +1,223 @@
+# -*- coding: utf-8 -*-
+from collections.abc import Iterable
+import numpy as np
+
+from scipy._lib._util import _asarray_validated
+from scipy.linalg import block_diag, LinAlgError
+from .lapack import _compute_lwork, get_lapack_funcs
+
+__all__ = ['cossin']
+
+
+def cossin(X, p=None, q=None, separate=False,
+           swap_sign=False, compute_u=True, compute_vh=True):
+    """
+    Compute the cosine-sine (CS) decomposition of an orthogonal/unitary matrix.
+
+    X is an ``(m, m)`` orthogonal/unitary matrix, partitioned as the following
+    where upper left block has the shape of ``(p, q)``::
+
+                                   ┌                   ┐
+                                   │ I  0  0 │ 0  0  0 │
+        ┌           ┐   ┌         ┐│ 0  C  0 │ 0 -S  0 │┌         ┐*
+        │ X11 │ X12 │   │ U1 │    ││ 0  0  0 │ 0  0 -I ││ V1 │    │
+        │ ────┼──── │ = │────┼────││─────────┼─────────││────┼────│
+        │ X21 │ X22 │   │    │ U2 ││ 0  0  0 │ I  0  0 ││    │ V2 │
+        └           ┘   └         ┘│ 0  S  0 │ 0  C  0 │└         ┘
+                                   │ 0  0  I │ 0  0  0 │
+                                   └                   ┘
+
+    ``U1``, ``U2``, ``V1``, ``V2`` are square orthogonal/unitary matrices of
+    dimensions ``(p,p)``, ``(m-p,m-p)``, ``(q,q)``, and ``(m-q,m-q)``
+    respectively, and ``C`` and ``S`` are ``(r, r)`` nonnegative diagonal
+    matrices satisfying ``C^2 + S^2 = I`` where ``r = min(p, m-p, q, m-q)``.
+
+    Moreover, the rank of the identity matrices are ``min(p, q) - r``,
+    ``min(p, m - q) - r``, ``min(m - p, q) - r``, and ``min(m - p, m - q) - r``
+    respectively.
+
+    X can be supplied either by itself and block specifications p, q or its
+    subblocks in an iterable from which the shapes would be derived. See the
+    examples below.
+
+    Parameters
+    ----------
+    X : array_like, iterable
+        complex unitary or real orthogonal matrix to be decomposed, or iterable
+        of subblocks ``X11``, ``X12``, ``X21``, ``X22``, when ``p``, ``q`` are
+        omitted.
+    p : int, optional
+        Number of rows of the upper left block ``X11``, used only when X is
+        given as an array.
+    q : int, optional
+        Number of columns of the upper left block ``X11``, used only when X is
+        given as an array.
+    separate : bool, optional
+        if ``True``, the low level components are returned instead of the
+        matrix factors, i.e. ``(u1,u2)``, ``theta``, ``(v1h,v2h)`` instead of
+        ``u``, ``cs``, ``vh``.
+    swap_sign : bool, optional
+        if ``True``, the ``-S``, ``-I`` block will be the bottom left,
+        otherwise (by default) they will be in the upper right block.
+    compute_u : bool, optional
+        if ``False``, ``u`` won't be computed and an empty array is returned.
+    compute_vh : bool, optional
+        if ``False``, ``vh`` won't be computed and an empty array is returned.
+
+    Returns
+    -------
+    u : ndarray
+        When ``compute_u=True``, contains the block diagonal orthogonal/unitary
+        matrix consisting of the blocks ``U1`` (``p`` x ``p``) and ``U2``
+        (``m-p`` x ``m-p``) orthogonal/unitary matrices. If ``separate=True``,
+        this contains the tuple of ``(U1, U2)``.
+    cs : ndarray
+        The cosine-sine factor with the structure described above.
+         If ``separate=True``, this contains the ``theta`` array containing the
+         angles in radians.
+    vh : ndarray
+        When ``compute_vh=True`, contains the block diagonal orthogonal/unitary
+        matrix consisting of the blocks ``V1H`` (``q`` x ``q``) and ``V2H``
+        (``m-q`` x ``m-q``) orthogonal/unitary matrices. If ``separate=True``,
+        this contains the tuple of ``(V1H, V2H)``.
+
+    Examples
+    --------
+    >>> from scipy.linalg import cossin
+    >>> from scipy.stats import unitary_group
+    >>> x = unitary_group.rvs(4)
+    >>> u, cs, vdh = cossin(x, p=2, q=2)
+    >>> np.allclose(x, u @ cs @ vdh)
+    True
+
+    Same can be entered via subblocks without the need of ``p`` and ``q``. Also
+    let's skip the computation of ``u``
+
+    >>> ue, cs, vdh = cossin((x[:2, :2], x[:2, 2:], x[2:, :2], x[2:, 2:]),
+    ...                      compute_u=False)
+    >>> print(ue)
+    []
+    >>> np.allclose(x, u @ cs @ vdh)
+    True
+
+    References
+    ----------
+    .. [1] : Brian D. Sutton. Computing the complete CS decomposition. Numer.
+           Algorithms, 50(1):33-65, 2009.
+
+    """
+
+    if p or q:
+        p = 1 if p is None else int(p)
+        q = 1 if q is None else int(q)
+        X = _asarray_validated(X, check_finite=True)
+        if not np.equal(*X.shape):
+            raise ValueError("Cosine Sine decomposition only supports square"
+                             " matrices, got {}".format(X.shape))
+        m = X.shape[0]
+        if p >= m or p <= 0:
+            raise ValueError("invalid p={}, 0<p<{} must hold"
+                             .format(p, X.shape[0]))
+        if q >= m or q <= 0:
+            raise ValueError("invalid q={}, 0<q<{} must hold"
+                             .format(q, X.shape[0]))
+
+        x11, x12, x21, x22 = X[:p, :q], X[:p, q:], X[p:, :q], X[p:, q:]
+    elif not isinstance(X, Iterable):
+        raise ValueError("When p and q are None, X must be an Iterable"
+                         " containing the subblocks of X")
+    else:
+        if len(X) != 4:
+            raise ValueError("When p and q are None, exactly four arrays"
+                             " should be in X, got {}".format(len(X)))
+
+        x11, x12, x21, x22 = [np.atleast_2d(x) for x in X]
+        for name, block in zip(["x11", "x12", "x21", "x22"],
+                               [x11, x12, x21, x22]):
+            if block.shape[1] == 0:
+                raise ValueError("{} can't be empty".format(name))
+        p, q = x11.shape
+        mmp, mmq = x22.shape
+
+        if x12.shape != (p, mmq):
+            raise ValueError("Invalid x12 dimensions: desired {}, "
+                             "got {}".format((p, mmq), x12.shape))
+
+        if x21.shape != (mmp, q):
+            raise ValueError("Invalid x21 dimensions: desired {}, "
+                             "got {}".format((mmp, q), x21.shape))
+
+        if p + mmp != q + mmq:
+            raise ValueError("The subblocks have compatible sizes but "
+                             "don't form a square array (instead they form a"
+                             " {}x{} array). This might be due to missing "
+                             "p, q arguments.".format(p + mmp, q + mmq))
+
+        m = p + mmp
+
+    cplx = any([np.iscomplexobj(x) for x in [x11, x12, x21, x22]])
+    driver = "uncsd" if cplx else "orcsd"
+    csd, csd_lwork = get_lapack_funcs([driver, driver + "_lwork"],
+                                      [x11, x12, x21, x22])
+    lwork = _compute_lwork(csd_lwork, m=m, p=p, q=q)
+    lwork_args = ({'lwork': lwork[0], 'lrwork': lwork[1]} if cplx else
+                  {'lwork': lwork})
+    *_, theta, u1, u2, v1h, v2h, info = csd(x11=x11, x12=x12, x21=x21, x22=x22,
+                                            compute_u1=compute_u,
+                                            compute_u2=compute_u,
+                                            compute_v1t=compute_vh,
+                                            compute_v2t=compute_vh,
+                                            trans=False, signs=swap_sign,
+                                            **lwork_args)
+
+    method_name = csd.typecode + driver
+    if info < 0:
+        raise ValueError('illegal value in argument {} of internal {}'
+                         .format(-info, method_name))
+    if info > 0:
+        raise LinAlgError("{} did not converge: {}".format(method_name, info))
+
+    if separate:
+        return (u1, u2), theta, (v1h, v2h)
+
+    U = block_diag(u1, u2)
+    VDH = block_diag(v1h, v2h)
+
+    # Construct the middle factor CS
+    c = np.diag(np.cos(theta))
+    s = np.diag(np.sin(theta))
+    r = min(p, q, m - p, m - q)
+    n11 = min(p, q) - r
+    n12 = min(p, m - q) - r
+    n21 = min(m - p, q) - r
+    n22 = min(m - p, m - q) - r
+    Id = np.eye(np.max([n11, n12, n21, n22, r]), dtype=theta.dtype)
+    CS = np.zeros((m, m), dtype=theta.dtype)
+
+    CS[:n11, :n11] = Id[:n11, :n11]
+
+    xs = n11 + r
+    xe = n11 + r + n12
+    ys = n11 + n21 + n22 + 2 * r
+    ye = n11 + n21 + n22 + 2 * r + n12
+    CS[xs: xe, ys:ye] = Id[:n12, :n12] if swap_sign else -Id[:n12, :n12]
+
+    xs = p + n22 + r
+    xe = p + n22 + r + + n21
+    ys = n11 + r
+    ye = n11 + r + n21
+    CS[xs:xe, ys:ye] = -Id[:n21, :n21] if swap_sign else Id[:n21, :n21]
+
+    CS[p:p + n22, q:q + n22] = Id[:n22, :n22]
+    CS[n11:n11 + r, n11:n11 + r] = c
+    CS[p + n22:p + n22 + r, r + n21 + n22:2 * r + n21 + n22] = c
+
+    xs = n11
+    xe = n11 + r
+    ys = n11 + n21 + n22 + r
+    ye = n11 + n21 + n22 + 2 * r
+    CS[xs:xe, ys:ye] = s if swap_sign else -s
+
+    CS[p + n22:p + n22 + r, n11:n11 + r] = -s if swap_sign else s
+
+    return U, CS, VDH
diff --git a/scipy/linalg/lapack.py b/scipy/linalg/lapack.py
--- a/scipy/linalg/lapack.py
+++ b/scipy/linalg/lapack.py
@@ -48,7 +48,6 @@
 .. autosummary::
    :toctree: generated/
 
-
    sgbsv
    dgbsv
    cgbsv
@@ -386,6 +385,11 @@
    clauum
    zlauum
 
+   sorcsd
+   dorcsd
+   sorcsd_lwork
+   dorcsd_lwork
+
    sorghr
    dorghr
    sorghr_lwork
@@ -712,6 +716,12 @@
    ctpmqrt
    ztpmqrt
 
+   cuncsd
+   zuncsd
+
+   cuncsd_lwork
+   zuncsd_lwork
+
    cunmrz
    zunmrz
 


Test patch:
diff --git a/scipy/linalg/tests/test_decomp_cossin.py b/scipy/linalg/tests/test_decomp_cossin.py
new file mode 100644
--- /dev/null
+++ b/scipy/linalg/tests/test_decomp_cossin.py
@@ -0,0 +1,154 @@
+import pytest
+import numpy as np
+from numpy.random import seed
+from numpy.testing import assert_allclose
+
+from scipy.linalg.lapack import _compute_lwork
+from scipy.stats import ortho_group, unitary_group
+from scipy.linalg import cossin, get_lapack_funcs, block_diag
+
+REAL_DTYPES = [np.float32, np.float64]
+COMPLEX_DTYPES = [np.complex64, np.complex128]
+DTYPES = REAL_DTYPES + COMPLEX_DTYPES
+
+
+@pytest.mark.parametrize('dtype_', DTYPES)
+@pytest.mark.parametrize('m, p, q',
+                         [
+                             (2, 1, 1),
+                             (3, 2, 1),
+                             (3, 1, 2),
+                             (4, 2, 2),
+                             (4, 1, 2),
+                             (40, 12, 20),
+                             (40, 30, 1),
+                             (40, 1, 30),
+                             (100, 50, 1),
+                             (100, 50, 50),
+                         ])
+@pytest.mark.parametrize('swap_sign', [True, False])
+def test_cossin(dtype_, m, p, q, swap_sign):
+    seed(1234)
+    if dtype_ in COMPLEX_DTYPES:
+        x = np.array(unitary_group.rvs(m), dtype=dtype_)
+    else:
+        x = np.array(ortho_group.rvs(m), dtype=dtype_)
+
+    u, cs, vh = cossin(x, p, q,
+                       swap_sign=swap_sign)
+    assert_allclose(x, u @ cs @ vh, rtol=0., atol=m*1e3*np.finfo(dtype_).eps)
+    assert u.dtype == dtype_
+    # Test for float32 or float 64
+    assert cs.dtype == np.real(u).dtype
+    assert vh.dtype == dtype_
+
+    u, cs, vh = cossin([x[:p, :q], x[:p, q:], x[p:, :q], x[p:, q:]],
+                       swap_sign=swap_sign)
+    assert_allclose(x, u @ cs @ vh, rtol=0., atol=m*1e3*np.finfo(dtype_).eps)
+    assert u.dtype == dtype_
+    assert cs.dtype == np.real(u).dtype
+    assert vh.dtype == dtype_
+
+    _, cs2, vh2 = cossin(x, p, q,
+                         compute_u=False,
+                         swap_sign=swap_sign)
+    assert_allclose(cs, cs2, rtol=0., atol=10*np.finfo(dtype_).eps)
+    assert_allclose(vh, vh2, rtol=0., atol=10*np.finfo(dtype_).eps)
+
+    u2, cs2, _ = cossin(x, p, q,
+                        compute_vh=False,
+                        swap_sign=swap_sign)
+    assert_allclose(u, u2, rtol=0., atol=10*np.finfo(dtype_).eps)
+    assert_allclose(cs, cs2, rtol=0., atol=10*np.finfo(dtype_).eps)
+
+    _, cs2, _ = cossin(x, p, q,
+                       compute_u=False,
+                       compute_vh=False,
+                       swap_sign=swap_sign)
+    assert_allclose(cs, cs2, rtol=0., atol=10*np.finfo(dtype_).eps)
+
+
+def test_cossin_mixed_types():
+    seed(1234)
+    x = np.array(ortho_group.rvs(4), dtype=np.float)
+    u, cs, vh = cossin([x[:2, :2],
+                        np.array(x[:2, 2:], dtype=np.complex128),
+                        x[2:, :2],
+                        x[2:, 2:]])
+
+    assert u.dtype == np.complex128
+    assert cs.dtype == np.float64
+    assert vh.dtype == np.complex128
+    assert_allclose(x, u @ cs @ vh, rtol=0.,
+                    atol=1e4 * np.finfo(np.complex128).eps)
+
+
+def test_cossin_error_incorrect_subblocks():
+    with pytest.raises(ValueError, match="be due to missing p, q arguments."):
+        cossin(([1, 2], [3, 4, 5], [6, 7], [8, 9, 10]))
+
+
+def test_cossin_error_empty_subblocks():
+    with pytest.raises(ValueError, match="x11.*empty"):
+        cossin(([], [], [], []))
+    with pytest.raises(ValueError, match="x12.*empty"):
+        cossin(([1, 2], [], [6, 7], [8, 9, 10]))
+    with pytest.raises(ValueError, match="x21.*empty"):
+        cossin(([1, 2], [3, 4, 5], [], [8, 9, 10]))
+    with pytest.raises(ValueError, match="x22.*empty"):
+        cossin(([1, 2], [3, 4, 5], [2], []))
+
+
+def test_cossin_error_missing_partitioning():
+    with pytest.raises(ValueError, match=".*exactly four arrays.* got 2"):
+        cossin(unitary_group.rvs(2))
+
+    with pytest.raises(ValueError, match=".*might be due to missing p, q"):
+        cossin(unitary_group.rvs(4))
+
+
+def test_cossin_error_non_iterable():
+    with pytest.raises(ValueError, match="containing the subblocks of X"):
+        cossin(12j)
+
+
+def test_cossin_error_non_square():
+    with pytest.raises(ValueError, match="only supports square"):
+        cossin(np.array([[1, 2]]), 1, 1)
+
+def test_cossin_error_partitioning():
+    x = np.array(ortho_group.rvs(4), dtype=np.float)
+    with pytest.raises(ValueError, match="invalid p=0.*0<p<4.*"):
+        cossin(x, 0, 1)
+    with pytest.raises(ValueError, match="invalid p=4.*0<p<4.*"):
+        cossin(x, 4, 1)
+    with pytest.raises(ValueError, match="invalid q=-2.*0<q<4.*"):
+        cossin(x, 1, -2)
+    with pytest.raises(ValueError, match="invalid q=5.*0<q<4.*"):
+        cossin(x, 1, 5)
+
+
+@pytest.mark.parametrize("dtype_", DTYPES)
+def test_cossin_separate(dtype_):
+    m, p, q = 250, 80, 170
+
+    pfx = 'or' if dtype_ in REAL_DTYPES else 'un'
+    X = ortho_group.rvs(m) if pfx == 'or' else unitary_group.rvs(m)
+    X = np.array(X, dtype=dtype_)
+
+    drv, dlw = get_lapack_funcs((pfx + 'csd', pfx + 'csd_lwork'),[X])
+    lwval = _compute_lwork(dlw, m, p, q)
+    lwvals = {'lwork': lwval} if pfx == 'or' else dict(zip(['lwork',
+                                                            'lrwork'],
+                                                           lwval))
+
+    *_, theta, u1, u2, v1t, v2t, _ = \
+        drv(X[:p, :q], X[:p, q:], X[p:, :q], X[p:, q:], **lwvals)
+
+    (u1_2, u2_2), theta2, (v1t_2, v2t_2) = cossin(X, p, q, separate=True)
+
+    assert_allclose(u1_2, u1, rtol=0., atol=10*np.finfo(dtype_).eps)
+    assert_allclose(u2_2, u2, rtol=0., atol=10*np.finfo(dtype_).eps)
+    assert_allclose(v1t_2, v1t, rtol=0., atol=10*np.finfo(dtype_).eps)
+    assert_allclose(v2t_2, v2t, rtol=0., atol=10*np.finfo(dtype_).eps)
+    assert_allclose(theta2, theta, rtol=0., atol=10*np.finfo(dtype_).eps)
diff --git a/scipy/linalg/tests/test_lapack.py b/scipy/linalg/tests/test_lapack.py
--- a/scipy/linalg/tests/test_lapack.py
+++ b/scipy/linalg/tests/test_lapack.py
@@ -22,9 +22,10 @@
 from numpy.random import rand, randint, seed
 
 from scipy.linalg import _flapack as flapack, lapack
-from scipy.linalg import inv, svd, cholesky, solve, ldl, norm
+from scipy.linalg import inv, svd, cholesky, solve, ldl, norm, block_diag, qr
 from scipy.linalg.lapack import _compute_lwork
-from scipy.linalg import qr
+from scipy.stats import ortho_group, unitary_group
+
 
 try:
     from scipy.linalg import _clapack as clapack
@@ -1776,3 +1777,64 @@ def test_generalized_eigh_lworks(pfx, driver):
     except Exception as e:
         pytest.fail("{}_lwork raised unexpected exception: {}"
                     "".format(pfx+driver, e))
+
+
+@pytest.mark.parametrize("dtype_", DTYPES)
+@pytest.mark.parametrize("m", [1, 10, 100, 1000])
+def test_orcsd_uncsd_lwork(dtype_, m):
+    seed(1234)
+    p = randint(0, m)
+    q = m - p
+    pfx = 'or' if dtype_ in REAL_DTYPES else 'un'
+    dlw = pfx + 'csd_lwork'
+    lw = get_lapack_funcs(dlw, dtype=dtype_)
+    lwval = _compute_lwork(lw, m, p, q)
+    lwval = lwval if pfx == 'un' else (lwval,)
+    assert all([x > 0 for x in lwval])
+
+
+@pytest.mark.parametrize("dtype_", DTYPES)
+def test_orcsd_uncsd(dtype_):
+    m, p, q = 250, 80, 170
+
+    pfx = 'or' if dtype_ in REAL_DTYPES else 'un'
+    X = ortho_group.rvs(m) if pfx == 'or' else unitary_group.rvs(m)
+
+    drv, dlw = get_lapack_funcs((pfx + 'csd', pfx + 'csd_lwork'), dtype=dtype_)
+    lwval = _compute_lwork(dlw, m, p, q)
+    lwvals = {'lwork': lwval} if pfx == 'or' else dict(zip(['lwork',
+                                                            'lrwork'], lwval))
+
+    cs11, cs12, cs21, cs22, theta, u1, u2, v1t, v2t, info =\
+        drv(X[:p, :q], X[:p, q:], X[p:, :q], X[p:, q:], **lwvals)
+
+    assert info == 0
+
+    U = block_diag(u1, u2)
+    VH = block_diag(v1t, v2t)
+    r = min(min(p, q), min(m-p, m-q))
+    n11 = min(p, q) - r
+    n12 = min(p, m-q) - r
+    n21 = min(m-p, q) - r
+    n22 = min(m-p, m-q) - r
+
+    S = np.zeros((m, m), dtype=dtype_)
+    one = dtype_(1.)
+    for i in range(n11):
+        S[i, i] = one
+    for i in range(n22):
+        S[p+i, q+i] = one
+    for i in range(n12):
+        S[i+n11+r, i+n11+r+n21+n22+r] = -one
+    for i in range(n21):
+        S[p+n22+r+i, n11+r+i] = one
+
+    for i in range(r):
+        S[i+n11, i+n11] = np.cos(theta[i])
+        S[p+n22+i, i+r+n21+n22] = np.cos(theta[i])
+
+        S[i+n11, i+n11+n21+n22+r] = -np.sin(theta[i])
+        S[p+n22+i, i+n11] = np.sin(theta[i])
+
+    Xc = U @ S @ VH
+    assert_allclose(X, Xc, rtol=0., atol=1e4*np.finfo(dtype_).eps)