From 3685b59c2372a2cbc3013f6f28b62ae80a0f074f Mon Sep 17 00:00:00 2001 From: Mateusz Baran Date: Wed, 7 Aug 2024 18:56:37 +0200 Subject: [PATCH] fixing special euclidean --- .../ManifoldsRecursiveArrayToolsExt.jl | 6 +++ .../special_euclidean_rat.jl | 42 ++++++++++++++----- src/groups/group.jl | 25 +++++++++-- src/groups/semidirect_product_group.jl | 12 ++++-- src/groups/special_euclidean.jl | 34 ++++++++++++--- test/groups/special_euclidean.jl | 28 +++++++++---- 6 files changed, 114 insertions(+), 33 deletions(-) diff --git a/ext/ManifoldsRecursiveArrayToolsExt/ManifoldsRecursiveArrayToolsExt.jl b/ext/ManifoldsRecursiveArrayToolsExt/ManifoldsRecursiveArrayToolsExt.jl index 5eea8dfaa..4dc35866a 100644 --- a/ext/ManifoldsRecursiveArrayToolsExt/ManifoldsRecursiveArrayToolsExt.jl +++ b/ext/ManifoldsRecursiveArrayToolsExt/ManifoldsRecursiveArrayToolsExt.jl @@ -14,9 +14,11 @@ if isdefined(Base, :get_extension) FiberBundleBasisData, FiberBundleProductVectorTransport, LeftColumnwiseSpecialEuclideanAction, + LeftInvariantRepresentation, PowerManifoldNestedReplacing, SpecialEuclideanIdentity, SpecialEuclideanInGeneralLinear, + TangentVectorRepresentation, TypeParameter using Manifolds: bundle_transport_tangent_direction, _get_parameter @@ -49,6 +51,7 @@ if isdefined(Base, :get_extension) project, translate, translate_diff, + vector_representation, _vector_transport_direction, vee else @@ -69,9 +72,11 @@ else FiberBundleBasisData, FiberBundleProductVectorTransport, LeftColumnwiseSpecialEuclideanAction, + LeftInvariantRepresentation, PowerManifoldNestedReplacing, SpecialEuclideanIdentity, SpecialEuclideanInGeneralLinear, + TangentVectorRepresentation, TypeParameter import ..Manifolds: @@ -102,6 +107,7 @@ else project, translate, translate_diff, + vector_representation, _vector_transport_direction, vee end diff --git a/ext/ManifoldsRecursiveArrayToolsExt/special_euclidean_rat.jl b/ext/ManifoldsRecursiveArrayToolsExt/special_euclidean_rat.jl index 381dd50a2..9c2517a0b 100644 --- a/ext/ManifoldsRecursiveArrayToolsExt/special_euclidean_rat.jl +++ b/ext/ManifoldsRecursiveArrayToolsExt/special_euclidean_rat.jl @@ -27,35 +27,51 @@ function project(M::SpecialEuclideanInGeneralLinear, p, X) G = M.manifold np, hp = submanifold_components(G, p) nX, hX = submanifold_components(G, X) - return ArrayPartition(hp * nX, hX) + if vector_representation(M.manifold) isa LeftInvariantRepresentation + return ArrayPartition(nX, hX) + else + return ArrayPartition(hp * nX, hX) + end end ### Special methods for better performance of selected operations -function Base.exp(M::SpecialEuclidean, p::ArrayPartition, X::ArrayPartition) +function Base.exp( + M::SpecialEuclidean{T,<:TangentVectorRepresentation}, + p::ArrayPartition, + X::ArrayPartition, +) where {T} M1, M2 = M.manifold.manifolds return ArrayPartition( exp(M1.manifold, p.x[1], X.x[1]), exp(M2.manifold, p.x[2], X.x[2]), ) end -function Base.log(M::SpecialEuclidean, p::ArrayPartition, q::ArrayPartition) +function Base.log( + M::SpecialEuclidean{T,<:TangentVectorRepresentation}, + p::ArrayPartition, + q::ArrayPartition, +) where {T} M1, M2 = M.manifold.manifolds return ArrayPartition( log(M1.manifold, p.x[1], q.x[1]), log(M2.manifold, p.x[2], q.x[2]), ) end -function vee(M::SpecialEuclidean, p::ArrayPartition, X::ArrayPartition) +function vee( + M::SpecialEuclidean{T,<:TangentVectorRepresentation}, + p::ArrayPartition, + X::ArrayPartition, +) where {T} M1, M2 = M.manifold.manifolds return vcat(vee(M1.manifold, p.x[1], X.x[1]), vee(M2.manifold, p.x[2], X.x[2])) end function get_coordinates( - M::SpecialEuclidean, + M::SpecialEuclidean{T,<:TangentVectorRepresentation}, p::ArrayPartition, X::ArrayPartition, basis::DefaultOrthogonalBasis, -) +) where {T} M1, M2 = M.manifold.manifolds return vcat( get_coordinates(M1.manifold, p.x[1], X.x[1], basis), @@ -63,7 +79,7 @@ function get_coordinates( ) end function hat( - M::SpecialEuclidean{TypeParameter{Tuple{2}}}, + M::SpecialEuclidean{TypeParameter{Tuple{2}},<:TangentVectorRepresentation}, p::ArrayPartition, c::AbstractVector, ) @@ -74,7 +90,7 @@ function hat( ) end function get_vector( - M::SpecialEuclidean{TypeParameter{Tuple{2}}}, + M::SpecialEuclidean{TypeParameter{Tuple{2}},<:TangentVectorRepresentation}, p::ArrayPartition, c::AbstractVector, basis::DefaultOrthogonalBasis, @@ -86,7 +102,7 @@ function get_vector( end function hat( - M::SpecialEuclidean{TypeParameter{Tuple{3}}}, + M::SpecialEuclidean{TypeParameter{Tuple{3}},<:TangentVectorRepresentation}, p::ArrayPartition, c::AbstractVector, ) @@ -97,7 +113,7 @@ function hat( ) end function get_vector( - M::SpecialEuclidean{TypeParameter{Tuple{3}}}, + M::SpecialEuclidean{TypeParameter{Tuple{3}},<:TangentVectorRepresentation}, p::ArrayPartition, c::AbstractVector, basis::DefaultOrthogonalBasis, @@ -107,6 +123,10 @@ function get_vector( get_vector(M.manifold.manifolds[2].manifold, p.x[2], c[SA[4, 5, 6]], basis), ) end -function compose(::SpecialEuclidean, p::ArrayPartition, q::ArrayPartition) +function compose( + ::SpecialEuclidean{T,<:TangentVectorRepresentation}, + p::ArrayPartition, + q::ArrayPartition, +) where {T} return ArrayPartition(p.x[2] * q.x[1] + p.x[1], p.x[2] * q.x[2]) end diff --git a/src/groups/group.jl b/src/groups/group.jl index 7c4bebe48..99fa2a9c9 100644 --- a/src/groups/group.jl +++ b/src/groups/group.jl @@ -1230,11 +1230,22 @@ end direction_and_side(::GroupExponentialRetraction{D}) where {D} = D() direction_and_side(::GroupLogarithmicInverseRetraction{D}) where {D} = D() -function log(::TraitList{<:IsGroupManifold}, G::AbstractDecoratorManifold, p, q) +function log( + ::TraitList{<:IsGroupManifold,<:LeftInvariantRepresentation}, + G::AbstractDecoratorManifold, + p, + q, +) BG = base_group(G) return log_lie(BG, compose(BG, inv(BG, p), q)) end -function log!(::TraitList{<:IsGroupManifold}, G::AbstractDecoratorManifold, X, p, q) +function log!( + ::TraitList{<:IsGroupManifold,<:LeftInvariantRepresentation}, + G::AbstractDecoratorManifold, + X, + p, + q, +) x = allocate_result(G, inv) BG = base_group(G) inv!(BG, x, p) @@ -1243,7 +1254,7 @@ function log!(::TraitList{<:IsGroupManifold}, G::AbstractDecoratorManifold, X, p return X end function exp( - ::TraitList{<:IsGroupManifold}, + ::TraitList{<:IsGroupManifold,<:LeftInvariantRepresentation}, G::AbstractDecoratorManifold, p, X, @@ -1252,7 +1263,13 @@ function exp( BG = base_group(G) return compose(BG, p, exp_lie(BG, t * X)) end -function exp!(::TraitList{<:IsGroupManifold}, G::AbstractDecoratorManifold, q, p, X) +function exp!( + ::TraitList{<:IsGroupManifold,<:LeftInvariantRepresentation}, + G::AbstractDecoratorManifold, + q, + p, + X, +) BG = base_group(G) exp_lie!(BG, q, X) compose!(BG, q, p, q) diff --git a/src/groups/semidirect_product_group.jl b/src/groups/semidirect_product_group.jl index b1906f7b9..3e020ac50 100644 --- a/src/groups/semidirect_product_group.jl +++ b/src/groups/semidirect_product_group.jl @@ -21,6 +21,9 @@ const SemidirectProductGroup{ GVR<:AbstractGroupVectorRepresentation, } = GroupManifold{𝔽,ProductManifold{𝔽,Tuple{N,H}},SemidirectProductOperation{A},GVR} +const SemidirectProductGroupTVR{𝔽,N,H,A<:AbstractGroupAction} = + SemidirectProductGroup{𝔽,N,H,A,TangentVectorRepresentation} + @doc raw""" SemidirectProductGroup(N::GroupManifold, H::GroupManifold, A::AbstractGroupAction) @@ -134,9 +137,10 @@ function _compose!(G::SemidirectProductGroup, x, p, q) end @doc raw""" - translate_diff(G::SemidirectProductGroup, p, q, X, conX::LeftForwardAction) + translate_diff(G::SemidirectProductGroupTVR, p, q, X, conX::LeftForwardAction) -Perform differential of the left translation on the semidirect product group `G`. +Perform differential of the left translation on the semidirect product group `G` +with `TangentVectorRepresentation`. Since the left translation is defined as (cf. [`SemidirectProductGroup`](@ref)): @@ -150,9 +154,9 @@ then its differential can be computed as \mathrm{d}L_{(n', h')}(X_n, X_h) = ( \mathrm{d}L_{n'} (\mathrm{d}θ_{h'}(X_n)), \mathrm{d}L_{h'} X_h). ```` """ -translate_diff(G::SemidirectProductGroup, p, q, X, conX::LeftForwardAction) +translate_diff(G::SemidirectProductGroupTVR, p, q, X, conX::LeftForwardAction) -function translate_diff!(G::SemidirectProductGroup, Y, p, q, X, conX::LeftForwardAction) +function translate_diff!(G::SemidirectProductGroupTVR, Y, p, q, X, conX::LeftForwardAction) M = base_manifold(G) N, H = M.manifolds A = G.op.action diff --git a/src/groups/special_euclidean.jl b/src/groups/special_euclidean.jl index 02c256d1a..08846910c 100644 --- a/src/groups/special_euclidean.jl +++ b/src/groups/special_euclidean.jl @@ -61,12 +61,23 @@ const SpecialEuclideanOperation{N} = SemidirectProductOperation{ } const SpecialEuclideanIdentity{N} = Identity{SpecialEuclideanOperation{N}} -function Base.show(io::IO, ::SpecialEuclidean{TypeParameter{Tuple{n}}}) where {n} - return print(io, "SpecialEuclidean($(n))") +function Base.show(io::IO, G::SpecialEuclidean{TypeParameter{Tuple{n}}}) where {n} + if vector_representation(G) isa TangentVectorRepresentation + return print(io, "SpecialEuclidean($(n))") + else + return print(io, "SpecialEuclidean($(n); gvr=LeftInvariantRepresentation())") + end end function Base.show(io::IO, G::SpecialEuclidean{Tuple{Int}}) n = _get_parameter(G) - return print(io, "SpecialEuclidean($(n); parameter=:field)") + if vector_representation(G) isa TangentVectorRepresentation + return print(io, "SpecialEuclidean($(n); parameter=:field)") + else + return print( + io, + "SpecialEuclidean($(n); parameter=:field, gvr=LeftInvariantRepresentation())", + ) + end end @inline function active_traits(f, M::SpecialEuclidean, args...) @@ -167,7 +178,7 @@ matrix part of `p`, `r` is the translation part of `fX` and `ω` is the rotation ``×`` is the cross product and ``⋅`` is the matrix product. """ function adjoint_action( - ::SpecialEuclidean{TypeParameter{Tuple{3}}}, + ::SpecialEuclidean{TypeParameter{Tuple{3}},<:TangentVectorRepresentation}, p, fX::TFVector{<:Any,VeeOrthogonalBasis{ℝ}}, ) @@ -642,7 +653,14 @@ is the translation part of `p` and ``X_t`` is the translation part of `X`. """ translate_diff(G::SpecialEuclidean, p, q, X, ::RightBackwardAction) -function translate_diff!(G::SpecialEuclidean, Y, p, q, X, ::RightBackwardAction) +function translate_diff!( + G::SpecialEuclidean{T,<:TangentVectorRepresentation}, + Y, + p, + q, + X, + ::RightBackwardAction, +) where {T} np, hp = submanifold_components(G, p) nq, hq = submanifold_components(G, q) nX, hX = submanifold_components(G, X) @@ -707,7 +725,11 @@ function embed(M::SpecialEuclideanInGeneralLinear, p, X) Y = allocate_result(G, screw_matrix, nX, hX) nY, hY = submanifold_components(G, Y) copyto!(hY, hX) - copyto!(nY, nX) + if vector_representation(M.manifold) isa LeftInvariantRepresentation + copyto!(nY, nX) + else + copyto!(nY, hp' * nX) + end @inbounds _padvector!(G, Y) return Y end diff --git a/test/groups/special_euclidean.jl b/test/groups/special_euclidean.jl index 51ddb8628..ec3cd7729 100644 --- a/test/groups/special_euclidean.jl +++ b/test/groups/special_euclidean.jl @@ -9,19 +9,29 @@ using Manifolds: LeftForwardAction, LeftBackwardAction, RightForwardAction, RightBackwardAction @testset "Special Euclidean group" begin - for se_parameter in [:field, :type] - @testset "SpecialEuclidean($n)" for n in (2, 3, 4) - G = SpecialEuclidean(n; parameter=se_parameter) + for (se_parameter, se_gvr) in [ + (:field, Manifolds.TangentVectorRepresentation()), + (:type, Manifolds.TangentVectorRepresentation()), + (:field, Manifolds.LeftInvariantRepresentation()), + ] + @testset "SpecialEuclidean($n; parameter=$se_parameter, gvr=$se_gvr)" for n in + (2, 3, 4) + G = SpecialEuclidean(n; parameter=se_parameter, gvr=se_gvr) if se_parameter === :field @test isa(G, SpecialEuclidean{Tuple{Int}}) else @test isa(G, SpecialEuclidean{TypeParameter{Tuple{n}}}) end - if se_parameter === :field + if se_parameter === :field && se_gvr === Manifolds.TangentVectorRepresentation() @test repr(G) == "SpecialEuclidean($n; parameter=:field)" - else + elseif se_parameter === :type && + se_gvr === Manifolds.TangentVectorRepresentation() @test repr(G) == "SpecialEuclidean($n)" + elseif se_parameter === :field && + se_gvr === Manifolds.LeftInvariantRepresentation() + @test repr(G) == + "SpecialEuclidean($n; parameter=:field, gvr=LeftInvariantRepresentation())" end M = base_manifold(G) @test M === @@ -92,9 +102,11 @@ using Manifolds: @test affine_matrix(G, Identity(G)) == SDiagonal{n,Float64}(I) w = translate_diff(G, pts[1], Identity(G), X_pts[1]) - w2mat = screw_matrix(G, w) - @test w2mat ≈ screw_matrix(G, X_pts[1]) - @test screw_matrix(G, w2mat) === w2mat + if se_gvr isa Manifolds.LeftInvariantRepresentation + w2mat = screw_matrix(G, w) + @test w2mat ≈ screw_matrix(G, X_pts[1]) + @test screw_matrix(G, w2mat) === w2mat + end @test is_vector(G, Identity(G), rand(G; vector_at=Identity(G)))