diff --git a/src/groups/semidirect_product_group.jl b/src/groups/semidirect_product_group.jl index f144bc7ebb..cd3942824d 100644 --- a/src/groups/semidirect_product_group.jl +++ b/src/groups/semidirect_product_group.jl @@ -173,7 +173,7 @@ end # We need to prevent decorator unwrapping so that the correct `get_vector!` gets called # and applies proper padding to the result if `X` happens to be a matrix. # Otherwise rare random bugs happen where the padding is not applied. -function get_vector(G::SemidirectProductGroup, p, X, B::VeeOrthogonalBasis) +function get_vector(G::SemidirectProductGroup, p, X, B::AbstractOrthogonalBasis) Y = allocate_result(G, get_vector, p, X) return get_vector!(G, Y, p, X, B) end diff --git a/src/groups/special_euclidean.jl b/src/groups/special_euclidean.jl index a5559d83e3..7d6d05409e 100644 --- a/src/groups/special_euclidean.jl +++ b/src/groups/special_euclidean.jl @@ -713,18 +713,52 @@ function vee(M::SpecialEuclidean, p::ArrayPartition, X::ArrayPartition) 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 hat(M::SpecialEuclidean{2}, p::ArrayPartition, c::SVector) +function get_coordinates( + M::SpecialEuclidean, + p::ArrayPartition, + X::ArrayPartition, + basis::AbstractBasis, +) + M1, M2 = M.manifold.manifolds + return vcat( + get_coordinates(M1.manifold, p.x[1], X.x[1], basis), + get_coordinates(M2.manifold, p.x[2], X.x[2], basis), + ) +end +function hat(M::SpecialEuclidean{2}, p::ArrayPartition, c) M1, M2 = M.manifold.manifolds return ArrayPartition( get_vector_orthogonal(M1.manifold, p.x[1], c[SOneTo(2)], ℝ), get_vector_orthogonal(M2.manifold, p.x[2], c[SA[3]], ℝ), ) end -function hat(M::SpecialEuclidean{3}, p::ArrayPartition, c::SVector) +function get_vector( + M::SpecialEuclidean{2}, + p::ArrayPartition, + c::AbstractVector, + basis::AbstractOrthogonalBasis, +) + return ArrayPartition( + get_vector(M.manifold.manifolds[1].manifold, p.x[1], view(c, 1:2), basis), + get_vector(M.manifold.manifolds[2].manifold, p.x[2], c[3], basis), + ) +end +function hat(M::SpecialEuclidean{3}, p::ArrayPartition, c) M1, M2 = M.manifold.manifolds return ArrayPartition( - get_vector_orthogonal(M1.manifold, p.x[1], c[SOneTo(3)], ℝ), - get_vector_orthogonal(M2.manifold, p.x[2], c[SA[4, 5, 6]], ℝ), + get_vector_orthogonal(M1.manifold, p.x[1], view(c, 1:3), ℝ), + get_vector_orthogonal(M2.manifold, p.x[2], view(c, 4:6), ℝ), + ) +end +function get_vector( + M::SpecialEuclidean{3}, + p::ArrayPartition, + Xc::AbstractVector, + basis::AbstractOrthogonalBasis, +) + return ArrayPartition( + get_vector(M.manifold.manifolds[1].manifold, p.x[1], view(Xc, 1:3), basis), + get_vector(M.manifold.manifolds[2].manifold, p.x[2], view(Xc, 4:6), basis), ) end function compose(::SpecialEuclidean, p::ArrayPartition, q::ArrayPartition)