diff --git a/src/groups/special_euclidean.jl b/src/groups/special_euclidean.jl index 12756f209d..a6c32ec4a9 100644 --- a/src/groups/special_euclidean.jl +++ b/src/groups/special_euclidean.jl @@ -725,7 +725,7 @@ function get_coordinates( get_coordinates(M2.manifold, p.x[2], X.x[2], basis), ) end -function hat(M::SpecialEuclidean{2}, p::ArrayPartition, c) +function hat(M::SpecialEuclidean{2}, p::ArrayPartition, c::AbstractVector) M1, M2 = M.manifold.manifolds return ArrayPartition( get_vector_orthogonal(M1.manifold, p.x[1], c[SOneTo(2)], ℝ), @@ -744,7 +744,7 @@ function get_vector( ) end -function hat(M::SpecialEuclidean{3}, p::ArrayPartition, c) +function hat(M::SpecialEuclidean{3}, p::ArrayPartition, c::AbstractVector) M1, M2 = M.manifold.manifolds return ArrayPartition( get_vector_orthogonal(M1.manifold, p.x[1], c[SOneTo(3)], ℝ), diff --git a/test/groups/special_euclidean.jl b/test/groups/special_euclidean.jl index d19c6df846..2200de7c6b 100644 --- a/test/groups/special_euclidean.jl +++ b/test/groups/special_euclidean.jl @@ -382,15 +382,29 @@ Random.seed!(10) @test isapprox(SEn, log(SEn, pts[1], pts[1]), 0 .* Xs[1]; atol=1e-16) @test isapprox(SEn, exp(SEn, pts[1], 0 .* Xs[1]), pts[1]) vee(SEn, pts[1], Xs[2]) + get_coordinates(SEn, pts[1], Xs[2], DefaultOrthogonalBasis()) csen = n == 2 ? SA[1.0, 2.0, 3.0] : SA[1.0, 0.0, 2.0, 2.0, -1.0, 1.0] hat(SEn, pts[1], csen) + get_vector(SEn, pts[1], csen, DefaultOrthogonalBasis()) # @btime shows 0 but `@allocations` is inaccurate @static if VERSION >= v"1.9-DEV" @test (@allocations exp(SEn, pts[1], Xs[1])) <= 4 @test (@allocations compose(SEn, pts[1], pts[2])) <= 4 @test (@allocations log(SEn, pts[1], pts[2])) <= 28 @test (@allocations vee(SEn, pts[1], Xs[2])) <= 13 + @test (@allocations get_coordinates( + SEn, + pts[1], + Xs[2], + DefaultOrthogonalBasis(), + )) <= 13 @test (@allocations hat(SEn, pts[1], csen)) <= 13 + @test (@allocations get_vector( + SEn, + pts[1], + csen, + DefaultOrthogonalBasis(), + )) <= 13 end end end