Propose the fix for #522.

src/groups/general_unitary_groups.jl

@@ -44,6 +44,108 @@ end
 
 decorated_manifold(G::GeneralUnitaryMultiplicationGroup) = G.manifold
 
+embed!(G::GeneralUnitaryMultiplicationGroup, Y, p, X) = copyto!(G, Y, p, X)
+
+@doc raw"""
+    exp(M::Rotations, p, X)
+    exp(M::OrthogonalMatrices, p, X)
+    exp(M::UnitaryMatrices, p, X)
+
+Compute the exponential map, that is, since ``X`` is represented in the Lie algebra,
+
+```
+exp_p(X) = p\mathrm{e}^X
+```
+
+For different sizes, like ``n=2,3,4`` there is specialised implementations
+
+The algorithm used is a more numerically stable form of those proposed in
+[^Gallier2002] and [^Andrica2013].
+
+[^Gallier2002]:
+    > Gallier J.; Xu D.; Computing exponentials of skew-symmetric matrices
+    > and logarithms of orthogonal matrices.
+    > International Journal of Robotics and Automation (2002), 17(4), pp. 1-11.
+    > [pdf](http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.35.3205).
+
+[^Andrica2013]:
+    > Andrica D.; Rohan R.-A.; Computing the Rodrigues coefficients of the
+    > exponential map of the Lie groups of matrices.
+    > Balkan Journal of Geometry and Its Applications (2013), 18(2), pp. 1-2.
+    > [pdf](https://www.emis.de/journals/BJGA/v18n2/B18-2-an.pdf).
+
+"""
+exp(::GeneralUnitaryMultiplicationGroup, p, X)
+
+function exp!(M::GeneralUnitaryMultiplicationGroup, q, p, X)
+    return copyto!(M, q, p * exp(X))
+end
+
+function exp(M::GeneralUnitaryMultiplicationGroup{2,ℝ}, p::SMatrix, X::SMatrix)
+    θ = get_coordinates(M, p, X, DefaultOrthogonalBasis())[1]
+    sinθ, cosθ = sincos(θ)
+    return p * SA[cosθ -sinθ; sinθ cosθ]
+end
+function exp!(M::GeneralUnitaryMultiplicationGroup{2,ℝ}, q, p, X)
+    @assert size(q) == (2, 2)
+    θ = get_coordinates(M, p, X, DefaultOrthogonalBasis())[1]
+    sinθ, cosθ = sincos(θ)
+    return copyto!(q, p * SA[cosθ -sinθ; sinθ cosθ])
+end
+function exp!(M::GeneralUnitaryMultiplicationGroup{3,ℝ}, q, p, X)
+    θ = norm(M, p, X) / sqrt(2)
+    if θ ≈ 0
+        a = 1 - θ^2 / 6
+        b = θ / 2
+    else
+        a = sin(θ) / θ
+        b = (1 - cos(θ)) / θ^2
+    end
+    pinvq = I + a .* X .+ b .* (X^2)
+    return copyto!(q, p * pinvq)
+end
+function exp!(::GeneralUnitaryMultiplicationGroup{4,ℝ}, q, p, X)
+    T = eltype(X)
+    α, β = angles_4d_skew_sym_matrix(X)
+    sinα, cosα = sincos(α)
+    sinβ, cosβ = sincos(β)
+    α² = α^2
+    β² = β^2
+    Δ = β² - α²
+    if !isapprox(Δ, 0; atol=1e-6)  # Case α > β ≥ 0
+        sincα = sinα / α
+        sincβ = β == 0 ? one(T) : sinβ / β
+        a₀ = (β² * cosα - α² * cosβ) / Δ
+        a₁ = (β² * sincα - α² * sincβ) / Δ
+        a₂ = (cosα - cosβ) / Δ
+        a₃ = (sincα - sincβ) / Δ
+    elseif α == 0 # Case α = β = 0
+        a₀ = one(T)
+        a₁ = one(T)
+        a₂ = T(1 / 2)
+        a₃ = T(1 / 6)
+    else  # Case α ⪆ β ≥ 0, α ≠ 0
+        sincα = sinα / α
+        r = β / α
+        c = 1 / (1 + r)
+        d = α * (α - β) / 2
+        if α < 1e-2
+            e = @evalpoly(α², T(1 / 3), T(-1 / 30), T(1 / 840), T(-1 / 45360))
+        else
+            e = (sincα - cosα) / α²
+        end
+        a₀ = (α * sinα + (1 + r - d) * cosα) * c
+        a₁ = ((3 - d) * sincα - (2 - r) * cosα) * c
+        a₂ = (sincα - (1 - r) / 2 * cosα) * c
+        a₃ = (e + (1 - r) * (e - sincα / 2)) * c
+    end
+
+    X² = X * X
+    X³ = X² * X
+    pinvq = a₀ * I + a₁ .* X .+ a₂ .* X² .+ a₃ .* X³
+    return copyto!(q, p * pinvq)
+end
+
 @doc raw"""
      exp_lie(G::Orthogonal{2}, X)
      exp_lie(G::SpecialOrthogonal{2}, X)
@@ -178,6 +280,101 @@ function log(G::GeneralUnitaryMultiplicationGroup, ::Identity{MultiplicationOper
     return log_lie(G, q)
 end
 
+@doc raw"""
+    log(M::Orthogonal, p, X)
+    log(M::SpecialOrthogonal, p, X)
+    log(M::SpecialUnitary, p, X)
+    log(M::Unitary, p, X)
+
+Compute the logarithmic map, that is, since the resulting ``X`` is represented in the Lie algebra,
+
+```
+log_p q = \log(p^{\mathrm{H}q)
+```
+which is projected onto the skew symmetric matrices for numerical stability.
+"""
+log(::GeneralUnitaryMultiplicationGroup, p, q)
+
+@doc raw"""
+    log(M::GeneralUnitaryMultiplicationGroup, p, q)
+
+Compute the logarithmic map on the [`Rotations`](@ref) manifold
+`M` which is given by
+
+```math
+\log_p q = \operatorname{log}(p^{\mathrm{T}}q)
+```
+
+where $\operatorname{Log}$ denotes the matrix logarithm. For numerical stability,
+the result is projected onto the set of skew symmetric matrices.
+
+For antipodal rotations the function returns deterministically one of the tangent vectors
+that point at `q`.
+"""
+log(::GeneralUnitaryMultiplicationGroup{n,ℝ}, ::Any...) where {n}
+function ManifoldsBase.log(M::GeneralUnitaryMultiplicationGroup{2,ℝ}, p, q)
+    U = transpose(p) * q
+    @assert size(U) == (2, 2)
+    @inbounds θ = atan(U[2], U[1])
+    return get_vector(M, p, θ, DefaultOrthogonalBasis())
+end
+function log!(::GeneralUnitaryMultiplicationGroup{n,ℝ}, X, p, q) where {n}
+    U = transpose(p) * q
+    X .= real(log_safe(U))
+    return project!(SkewSymmetricMatrices(n), X, p, X)
+end
+function log!(M::GeneralUnitaryMultiplicationGroup{2,ℝ}, X, p, q)
+    U = transpose(p) * q
+    @assert size(U) == (2, 2)
+    @inbounds θ = atan(U[2], U[1])
+    return get_vector!(M, X, p, θ, DefaultOrthogonalBasis())
+end
+function log!(M::GeneralUnitaryMultiplicationGroup{3,ℝ}, X, p, q)
+    U = transpose(p) * q
+    cosθ = (tr(U) - 1) / 2
+    if cosθ ≈ -1
+        eig = eigen_safe(U)
+        ival = findfirst(λ -> isapprox(λ, 1), eig.values)
+        inds = SVector{3}(1:3)
+        ax = eig.vectors[inds, ival]
+        return get_vector!(M, X, p, π * ax, DefaultOrthogonalBasis())
+    end
+    X .= U ./ usinc_from_cos(cosθ)
+    return project!(SkewSymmetricMatrices(3), X, p, X)
+end
+function log!(::GeneralUnitaryMultiplicationGroup{4,ℝ}, X, p, q)
+    U = transpose(p) * q
+    cosα, cosβ = Manifolds.cos_angles_4d_rotation_matrix(U)
+    α = acos(clamp(cosα, -1, 1))
+    β = acos(clamp(cosβ, -1, 1))
+    if α ≈ 0 && β ≈ π
+        A² = Symmetric((U - I) ./ 2)
+        P = eigvecs(A²)
+        E = similar(U)
+        fill!(E, 0)
+        @inbounds begin
+            E[2, 1] = -β
+            E[1, 2] = β
+        end
+        copyto!(X, P * E * transpose(P))
+    else
+        det(U) < 0 && throw(
+            DomainError(
+                "The logarithm is not defined for $p and $q with a negative determinant of p'q) ($(det(U)) < 0).",
+            ),
+        )
+        copyto!(X, real(Manifolds.log_safe(U)))
+    end
+    return project!(SkewSymmetricMatrices(4), X, p, X)
+end
+
+function log!(::GeneralUnitaryMultiplicationGroup{n,𝔽}, X, p, q) where {n,𝔽}
+    log_safe!(X, adjoint(p) * q)
+    project!(SkewHermitianMatrices(n, 𝔽), X, q)
+    X .= p * X
+    return X
+end
+
 function log!(
     G::GeneralUnitaryMultiplicationGroup,
     X,
@@ -258,6 +455,27 @@ function log_lie!(
     return project!(G, X, Identity(G), X)
 end
 
+@doc raw"""
+     project(M::Orthogonal{n}, p, X)
+     project(M::SpecialUnitary{n}, p, X)
+     project(M::SpecialOrthogonal{n}, p, X)
+     project(M::SpecialUnitary{n}, p, X)
+
+Orthogonally project the tangent vector ``X ∈ 𝔽^{n × n}``, ``\mathbb F ∈ \{\mathbb R, \mathbb C\}``
+to the tangent space of `M` at `p`,
+where `X` is already assumed to be in a Lie algebra represemtation, that isand change the representer to use the corresponding Lie algebra, i.e. we compute
+
+```math
+    \operatorname{proj}_p(X) = \frac{X-X^{\mathrm{T}}}{2},
+```
+"""
+project(::GeneralUnitaryMultiplicationGroup, p, X)
+
+function project!(::GeneralUnitaryMultiplicationGroup{n,𝔽}, Y, p, X) where {n,𝔽}
+    project!(SkewHermitianMatrices(n, 𝔽), Y, X)
+    return Y
+end
+
 function Random.rand!(G::GeneralUnitaryMultiplicationGroup, pX; kwargs...)
     rand!(G.manifold, pX; kwargs...)
     return pX