Add more SE2 and SE3 performance dispatches

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

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)