From c33013eb3c72c0399259d18d4c2a85c2adf58641 Mon Sep 17 00:00:00 2001 From: Mateusz Baran Date: Wed, 13 Nov 2024 12:20:44 +0100 Subject: [PATCH] Adjoint matrix --- NEWS.md | 6 +++++ Project.toml | 4 ++-- src/Manifolds.jl | 4 ++++ src/groups/group.jl | 27 +++++++++++++++++++++ src/groups/special_euclidean.jl | 30 ++++++++++++++++++++++++ src/groups/special_orthogonal.jl | 22 +++++++++++++++-- test/groups/groups_general.jl | 11 +++++++++ test/groups/special_euclidean.jl | 39 +++++++++++++++++++++++++++++++ test/groups/special_orthogonal.jl | 23 ++++++++++++++++++ 9 files changed, 162 insertions(+), 4 deletions(-) diff --git a/NEWS.md b/NEWS.md index d5e53e5ae6..ff26dd9a55 100644 --- a/NEWS.md +++ b/NEWS.md @@ -5,6 +5,12 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/), and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0.html). +## [0.10.7] – unreleased + +### Added + +* `adjoint_matrix` for Lie groups, with optimized implementations for SO(2), SO(3), SE(2) and SE(3). + ## [0.10.6] – 2024-11-06 ### Added diff --git a/Project.toml b/Project.toml index 42c6a802ae..033aa9bfa1 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "Manifolds" uuid = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" authors = ["Seth Axen ", "Mateusz Baran ", "Ronny Bergmann ", "Antoine Levitt "] -version = "0.10.6" +version = "0.10.7" [deps] Einsum = "b7d42ee7-0b51-5a75-98ca-779d3107e4c0" @@ -54,7 +54,7 @@ Graphs = "1.4" HybridArrays = "0.4" Kronecker = "0.4, 0.5" LinearAlgebra = "1.6" -ManifoldDiff = "0.3.7" +ManifoldDiff = "0.3.13" ManifoldsBase = "0.15.18" Markdown = "1.6" MatrixEquations = "2.2" diff --git a/src/Manifolds.jl b/src/Manifolds.jl index 2b36221c28..1831f8c7f2 100644 --- a/src/Manifolds.jl +++ b/src/Manifolds.jl @@ -1029,6 +1029,8 @@ export adjoint_action, adjoint_apply_diff_group!, adjoint_inv_diff, adjoint_inv_diff!, + adjoint_matrix, + adjoint_matrix!, affine_matrix, apply, apply!, @@ -1077,6 +1079,8 @@ export adjoint_action, inverse_translate!, inverse_translate_diff, inverse_translate_diff!, + jacobian_inv, + jacobian_inv!, lie_bracket, lie_bracket!, log_inv, diff --git a/src/groups/group.jl b/src/groups/group.jl index 5ec6888e96..468d0cb658 100644 --- a/src/groups/group.jl +++ b/src/groups/group.jl @@ -538,6 +538,33 @@ end @trait_function adjoint_inv_diff!(G::AbstractDecoratorManifold, Y, p, X) +""" + adjoint_matrix(G::AbstractManifold, p, B::AbstractBasis=DefaultOrthonormalBasis()) + +Compute the adjoint matrix related to conjugation of vectors by element `p` of Lie group `G` +for basis `B`. It is the matrix ``A`` such that for each element `X` of the Lie algebra +with coefficients ``c`` in basis `B`, ``Ac`` is the vector of coefficients of `X` conjugated +by `p` in basis `B`. +""" +function adjoint_matrix(G::AbstractManifold, p, B::AbstractBasis=DefaultOrthonormalBasis()) + J = ManifoldDiff.allocate_jacobian(G, G, adjoint_matrix, p) + return adjoint_matrix!(G, J, p, B) +end + +function adjoint_matrix!( + G::AbstractManifold, + J, + p, + B::AbstractBasis=DefaultOrthonormalBasis(), +) + Bb = get_basis(G, p, B) + Vs = get_vectors(G, p, Bb) + for i in eachindex(Vs) + get_coordinates!(G, view(J, :, i), p, adjoint_action(G, p, Vs[i]), B) + end + return J +end + function ManifoldDiff.differential_exp_argument_lie_approx!( M::AbstractManifold, Z, diff --git a/src/groups/special_euclidean.jl b/src/groups/special_euclidean.jl index d84559948d..d092adc1b3 100644 --- a/src/groups/special_euclidean.jl +++ b/src/groups/special_euclidean.jl @@ -190,6 +190,36 @@ function adjoint_action( return TFVector([cross(t, Rω) + R * r; Rω], fX.basis) end +@doc raw""" + adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{2}}}, p) + +Compute the adjoint matrix for the group [`SpecialEuclidean`](@ref)`(2)` at point `p` +in default coordinates. The formula follows Section 10.6.2 in [Chirikjian:2012] +but with additional scaling by ``\sqrt(2)`` due to a different choice of inner product. +""" +function adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{2}}}, p) + t, R = submanifold_components(p) + return @SMatrix [ + R[1, 1] R[1, 2] t[2]/sqrt(2) + R[2, 1] R[2, 2] -t[1]/sqrt(2) + 0 0 1 + ] +end +@doc raw""" + adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{3}}}, p) + +Compute the adjoint matrix for the group [`SpecialEuclidean`](@ref)`(3)` at point `p` +in default coordinates. The formula follows Section 10.6.9 in [Chirikjian:2012] with +changes due to different conventions. +""" +function adjoint_matrix(::SpecialEuclidean{TypeParameter{Tuple{3}}}, p) + t, R = submanifold_components(p) + Z = @SMatrix zeros(3, 3) + c = sqrt(2) \ @SMatrix [0 -t[3] t[2]; t[3] 0 -t[1]; -t[2] t[1] 0] + U = c * R + return vcat(hcat(R, U), hcat(Z, R)) +end + @doc raw""" affine_matrix(G::SpecialEuclidean, p) -> AbstractMatrix diff --git a/src/groups/special_orthogonal.jl b/src/groups/special_orthogonal.jl index e5d30cae13..8cccec06b6 100644 --- a/src/groups/special_orthogonal.jl +++ b/src/groups/special_orthogonal.jl @@ -1,17 +1,35 @@ @doc raw""" - SpecialOrthogonal{n} = GeneralUnitaryMultiplicationGroup{n,ℝ,DeterminantOneMatrices} + SpecialOrthogonal{T} = GeneralUnitaryMultiplicationGroup{T,ℝ,DeterminantOneMatrices} Special orthogonal group ``\mathrm{SO}(n)`` represented by rotation matrices, see [`Rotations`](@ref). # Constructor SpecialOrthogonal(n) """ -const SpecialOrthogonal{n} = GeneralUnitaryMultiplicationGroup{n,ℝ,DeterminantOneMatrices} +const SpecialOrthogonal{T} = GeneralUnitaryMultiplicationGroup{T,ℝ,DeterminantOneMatrices} function SpecialOrthogonal(n; parameter::Symbol=:type) return GeneralUnitaryMultiplicationGroup(Rotations(n; parameter=parameter)) end +""" + adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{2}}}, p) + +Compte the adjoint matrix for [`SpecialOrthogonal`](@ref)`(2)` at point `p`, which is equal +to `1`. See [SolaDerayAtchuthan:2021], Appendix A. +""" +adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{2}}}, p) = @SMatrix [1] +""" + adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{3}}}, p) + +Compte the adjoint matrix for [`SpecialOrthogonal`](@ref)`(3)` at point `p`, which is equal +to `p`. See [Chirikjian:2012](@cite), Section 10.6.6. +""" +adjoint_matrix(::SpecialOrthogonal{TypeParameter{Tuple{3}}}, p) = p + +adjoint_matrix!(::SpecialOrthogonal{TypeParameter{Tuple{2}}}, J, p) = fill!(J, 1) +adjoint_matrix!(::SpecialOrthogonal{TypeParameter{Tuple{3}}}, J, p) = copyto!(J, p) + Base.inv(::SpecialOrthogonal, p) = transpose(p) Base.inv(::SpecialOrthogonal, e::Identity{MultiplicationOperation}) = e diff --git a/test/groups/groups_general.jl b/test/groups/groups_general.jl index a48f8fc23d..40c34f811c 100644 --- a/test/groups/groups_general.jl +++ b/test/groups/groups_general.jl @@ -322,6 +322,17 @@ using Manifolds: end end end + + @testset "Jacobians" begin + M = SpecialOrthogonal(3) + p = [ + -0.333167290022488 -0.7611396995437196 -0.5564763378954822 + 0.8255218425902797 0.049666662385985494 -0.5621804959741897 + 0.4555362161951786 -0.646683524164589 0.6117901399243428 + ] + # testing the fallback definition just in case + @test invoke(adjoint_matrix, Tuple{AbstractManifold,Any}, M, p) ≈ p + end end struct NotImplementedAction <: AbstractGroupAction{LeftAction} end diff --git a/test/groups/special_euclidean.jl b/test/groups/special_euclidean.jl index d063fdc98d..73931a54e0 100644 --- a/test/groups/special_euclidean.jl +++ b/test/groups/special_euclidean.jl @@ -492,4 +492,43 @@ using Manifolds: end end end + + @testset "Jacobians" begin + M = SpecialEuclidean(2) + p = ArrayPartition( + [-0.6205177383168391, 0.9437210292185024], + [ + -0.5506838288169109 0.834713915470173 + -0.834713915470173 -0.5506838288169109 + ], + ) + + Jref = [ + -0.5506838288169109 0.834713915470173 0.667311539308751 + -0.834713915470173 -0.5506838288169109 0.4387723006103765 + 0.0 0.0 1.0 + ] + @test adjoint_matrix(M, p) ≈ Jref + + M = SpecialEuclidean(3) + p = ArrayPartition( + [0.1124045202347309, -0.4336604812325255, 0.8520978475672548], + [ + 0.590536813431926 0.6014916127888292 -0.538027984148 + -0.7691833864513029 0.21779306754302752 -0.6007687556269085 + -0.24417860264358274 0.7686182534099043 0.5912721797413909 + ], + ) + + Jref = [ + 0.590536813431926 0.6014916127888292 -0.538027984148 0.5383275472420492 -0.3669179673652647 0.18066746943124343 + -0.7691833864513029 0.21779306754302752 -0.6007687556269085 0.375220504480154 0.3013219176415302 -0.37117035706729606 + -0.24417860264358274 0.7686182534099043 0.5912721797413909 0.11994849553498667 0.20175458298403887 -0.21273349816106235 + 0.0 0.0 0.0 0.5905368134319258 0.6014916127888291 -0.5380279841480001 + 0.0 0.0 0.0 -0.7691833864513032 0.21779306754302766 -0.6007687556269088 + 0.0 0.0 0.0 -0.24417860264358288 0.7686182534099045 0.5912721797413911 + ] + + @test adjoint_matrix(M, p) ≈ Jref + end end diff --git a/test/groups/special_orthogonal.jl b/test/groups/special_orthogonal.jl index 32753dd0bf..59d1eccf2f 100644 --- a/test/groups/special_orthogonal.jl +++ b/test/groups/special_orthogonal.jl @@ -224,4 +224,27 @@ using Manifolds: LeftForwardAction, RightBackwardAction atol=1e-12, ) end + + @testset "Jacobians" begin + M = SpecialOrthogonal(2) + p = [ + -0.7689185267244146 -0.6393467754356441 + 0.6393467754356439 -0.7689185267244146 + ] + @test adjoint_matrix(M, p) == [1;;] + J = [0.0;;] + adjoint_matrix!(M, J, p) + @test J == [1;;] + + M = SpecialOrthogonal(3) + p = [ + -0.333167290022488 -0.7611396995437196 -0.5564763378954822 + 0.8255218425902797 0.049666662385985494 -0.5621804959741897 + 0.4555362161951786 -0.646683524164589 0.6117901399243428 + ] + @test adjoint_matrix(M, p) == p + J = similar(p) + adjoint_matrix!(M, J, p) + @test J == p + end end