diff --git a/NEWS.md b/NEWS.md index 756dea35..96e0a979 100644 --- a/NEWS.md +++ b/NEWS.md @@ -5,6 +5,18 @@ All notable Changes to the Julia package `LieGroups.jl` will be documented in th 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.1.12] unreleased + +### Added + +* `jacobian_exp` for the `TranslationGroup` (identity), the `SpecialEuclideanGroup` in both variants and dimensions 2 and 3, and the `SpecialGalileanGroup(3)` (closed form from Kelly, arXiv:2312.07555, section 8). All closed forms fall back to numerically robust expansions near zero rotation angles. +* `jacobian_exp` for a (direct) `ProductLieGroup`, assembled block-diagonally from the `jacobian_exp` of its factors. +* Fix ambiguities with `ZeroVector`and `Identity{AdditionGroupOperation}` in addition operations. `ZeroVector` was introduced in ManifoldsBase v2.5. + +### Changed + +* `jacobian_exp(G, g, X, b)` and `jacobian_exp!(G, J, g, X, b)` no longer take the base point `g`, since the Jacobian of the group exponential map does not depend on it: use `jacobian_exp(G, X, b)` and `jacobian_exp!(G, J, X, b)` instead. The old signatures are deprecated and still work. + ## [0.1.11] 2026-05-05 ### Added diff --git a/Project.toml b/Project.toml index 63a30950..01735310 100644 --- a/Project.toml +++ b/Project.toml @@ -22,7 +22,7 @@ LieGroupsTestExt = "Test" [compat] LinearAlgebra = "1.10" Manifolds = "0.11" -ManifoldsBase = "2" +ManifoldsBase = "2.5" Quaternions = "0.7.6" Random = "1.10" RecursiveArrayTools = "2, 3, 4.0" diff --git a/docs/src/groups/special_euclidean_group.md b/docs/src/groups/special_euclidean_group.md index 241f8450..6fc5db58 100644 --- a/docs/src/groups/special_euclidean_group.md +++ b/docs/src/groups/special_euclidean_group.md @@ -16,7 +16,7 @@ SpecialEuclideanProductTangentVector # Specific functions ```@autodocs -Modules = [LieGroups] -Pages = ["groups/special_euclidean_group.jl"] +Modules = [LieGroups, Base.get_extension(LieGroups, :LieGroupsRecursiveArrayToolsExt)] +Pages = ["groups/special_euclidean_group.jl", "ext/LieGroupsRecursiveArrayToolsExt/special_euclidean_group_RAT_ext.jl"] Order = [:function] ``` \ No newline at end of file diff --git a/docs/src/groups/special_galilean_group.md b/docs/src/groups/special_galilean_group.md index fadd04cc..a8fb3c9c 100644 --- a/docs/src/groups/special_galilean_group.md +++ b/docs/src/groups/special_galilean_group.md @@ -1,7 +1,7 @@ # The special Galilean group ```@autodocs -Modules = [LieGroups] -Pages = ["groups/special_galilean_group.jl"] +Modules = [LieGroups, Base.get_extension(LieGroups, :LieGroupsRecursiveArrayToolsExt)] +Pages = ["groups/special_galilean_group.jl", "ext/LieGroupsRecursiveArrayToolsExt/special_galilean_group_RAT_ext.jl"] Order = [:type, :function] ``` \ No newline at end of file diff --git a/docs/src/tutorials/transition.md b/docs/src/tutorials/transition.md index bb7e130b..616639c2 100644 --- a/docs/src/tutorials/transition.md +++ b/docs/src/tutorials/transition.md @@ -61,7 +61,7 @@ The list lists first types, then functions. Within both blocks, the order is alp | `inverse_translate(G, g, h, c)` | [`inv_left_compose`](@ref)`(G, g, h)`, [`inv_right_compose`](@ref)`(G, g, h)` | compute ``g^{-1}∘h`` and ``g∘h^{-1}``, resp. | | `inverse_translate_diff(G, g, h, X, LeftForwardAction())` | - | discontinued, use `diff_left_compose(G, inv(G,g), h)` | | `inverse_translate_diff(G, g, h, X, RightBackwardAction())` | - | discontinued, use `diff_left_compose(G, h, inv(G,g))` | -| `jacobian_exp_argument(G, g, X, b)` | [`jacobian_exp`](@ref)`(G, g, X, b)` | the Jacobian of the exponential map w.r.t. an [`AbstractBasis`](@extref `ManifoldsBase.AbstractBasis`) of the [`LieAlgebra`](@ref). The old name is resevered for the Riemannian exponential map. | +| `jacobian_exp_argument(G, g, X, b)` | [`jacobian_exp`](@ref)`(G, X, b)` | the Jacobian of the exponential map w.r.t. an [`AbstractBasis`](@extref `ManifoldsBase.AbstractBasis`) of the [`LieAlgebra`](@ref). The old name is resevered for the Riemannian exponential map. | | `log(G, g, h)` | `log(`[`base_manifold`](@ref base_manifold(G::LieGroup))`(G), g, h)` | you can now access the previous defaults on the internal manifold whenever they do not agree with the invariant one | | `log_inv(G, g, h)` | [`log`](@ref log(G::LieGroup, g, h))`(G, g, h)` | the logarithmic map invariant to the group operation is the default on Lie groups here | | `log_lie(G, g)` | [`log`](@ref log(G::LieGroup, g))`(G, g)` | the (matrix/Lie group) logarithm | diff --git a/ext/LieGroupsRecursiveArrayToolsExt/special_euclidean_group_RAT_ext.jl b/ext/LieGroupsRecursiveArrayToolsExt/special_euclidean_group_RAT_ext.jl index 97d7229f..4148bb5e 100644 --- a/ext/LieGroupsRecursiveArrayToolsExt/special_euclidean_group_RAT_ext.jl +++ b/ext/LieGroupsRecursiveArrayToolsExt/special_euclidean_group_RAT_ext.jl @@ -232,6 +232,33 @@ function ManifoldsBase.log!( return X end +function LieGroups.jacobian_exp!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{2}}}, + J::AbstractMatrix, + X::ArrayPartition, + ::DefaultLieAlgebraOrthogonalBasis, + ) + return LieGroups._jacobian_exp_SE2!(G, J, X) +end + +function LieGroups.jacobian_exp!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{3}}}, + J::AbstractMatrix, + X::ArrayPartition, + ::DefaultLieAlgebraOrthogonalBasis, + ) + return LieGroups._jacobian_exp_SE3!(G, J, X) +end + +function LieGroups.jacobian_exp!( + G::SpecialEuclideanGroup, + J::AbstractMatrix, + X::SpecialEuclideanProductTangentVector, + B::DefaultLieAlgebraOrthogonalBasis, + ) + return LieGroups.jacobian_exp!(G, J, ManifoldsBase.internal_value(X), B) +end + function LinearAlgebra.norm( 𝔤::LieAlgebra{ ℝ, <:LieGroups.SpecialEuclideanGroupOperation, <:LieGroups.SpecialEuclideanGroup, diff --git a/ext/LieGroupsRecursiveArrayToolsExt/special_galilean_group_RAT_ext.jl b/ext/LieGroupsRecursiveArrayToolsExt/special_galilean_group_RAT_ext.jl index 8f96917c..5d6d266d 100644 --- a/ext/LieGroupsRecursiveArrayToolsExt/special_galilean_group_RAT_ext.jl +++ b/ext/LieGroupsRecursiveArrayToolsExt/special_galilean_group_RAT_ext.jl @@ -1,13 +1,7 @@ -using LieGroups: SpecialGalileanGroup +using LieGroups: SpecialGalileanGroup, _skew using StaticArrays using LinearAlgebra -# Internal function to compute the skew-symmetric matrix as an SMatrix used for performance. -# Can be replaced with hat(SO(3), v) once that works without allocations. -function _skew(v::AbstractVector{T}) where {T <: Real} - return SMatrix{3, 3, T}(0, v[3], -v[2], -v[3], 0, v[1], v[2], -v[1], 0) -end - # Internal function to compute the matrix Q used in the exponential and logarithm maps for the Special Galilean group. (D matrix in Kelly:2025) function _Q(θ⃗) T = eltype(θ⃗) @@ -200,6 +194,61 @@ function LieGroups.compose(::SpecialGalileanGroup, g::ArrayPartition, h::ArrayPa ) end +_doc_lie_bracket_SGal3 = """ + lie_bracket(𝔰𝔤𝔞𝔩::LieAlgebra{ℝ,<:LeftSpecialGalileanGroupOperation,<:SpecialGalileanGroup}, X, Y) + lie_bracket!(𝔰𝔤𝔞𝔩::LieAlgebra{ℝ,<:LeftSpecialGalileanGroupOperation,<:SpecialGalileanGroup}, Z, X, Y) + +Compute the Lie bracket ``[X, Y] = XY - YX`` of two tangent vectors `X`, `Y` of the Lie algebra of the +[`SpecialGalileanGroup`](@ref)`(3)`, i.e. the matrix commutator of their ``5×5`` `hat` representations. + +In the ``((\\Omega, \\nu), (\\rho, \\iota))`` block form (see [`hat`](@ref)) this reduces to +```math +[X, Y] = \\bigl( +(\\Omega_X \\Omega_Y - \\Omega_Y \\Omega_X,\\ \\Omega_X \\nu_Y - \\Omega_Y \\nu_X),\\ +(\\Omega_X \\rho_Y - \\Omega_Y \\rho_X + \\iota_Y \\nu_X - \\iota_X \\nu_Y,\\ 0) +\\bigr). +``` +The basis is defined in eq. (14) of [Kelly:2025](@cite). + +This can be computed in-place of `Z`. +""" + +"$(_doc_lie_bracket_SGal3)" +function LieGroups.lie_bracket( + ::typeof(LieAlgebra(SpecialGalileanGroup(3))), + X::ArrayPartition, + Y::ArrayPartition, + ) + ΩX, νX, ρX, ιX = X.x[1].x[1], X.x[1].x[2], X.x[2].x[1], X.x[2].x[2][1] + ΩY, νY, ρY, ιY = Y.x[1].x[1], Y.x[1].x[2], Y.x[2].x[1], Y.x[2].x[2][1] + return ArrayPartition( + ArrayPartition( + ΩX * ΩY - ΩY * ΩX, # Ω + ΩX * νY - ΩY * νX, # ν + ), + ArrayPartition( + ΩX * ρY - ΩY * ρX + ιY * νX - ιX * νY, # ρ + zero(X.x[2].x[2]), # ι + ), + ) +end + +"$(_doc_lie_bracket_SGal3)" +function LieGroups.lie_bracket!( + ::typeof(LieAlgebra(SpecialGalileanGroup(3))), + Z::ArrayPartition, + X::ArrayPartition, + Y::ArrayPartition, + ) + ΩX, νX, ρX, ιX = X.x[1].x[1], X.x[1].x[2], X.x[2].x[1], X.x[2].x[2][1] + ΩY, νY, ρY, ιY = Y.x[1].x[1], Y.x[1].x[2], Y.x[2].x[1], Y.x[2].x[2][1] + Z.x[1].x[1] .= ΩX * ΩY .- ΩY * ΩX + Z.x[1].x[2] .= ΩX * νY .- ΩY * νX + Z.x[2].x[1] .= ΩX * ρY .- ΩY * ρX .+ ιY .* νX .- ιX .* νY + Z.x[2].x[2] .= 0 + return Z +end + # Dev NOTE: hat and vee use a different bases order than that of the underlining semidirect + direct product groups, # therefore, get_vector_lie and get_coordinates_lie are implemented explicitly. see hat/vee docstrings for details. function LieGroups.get_vector_lie( @@ -285,3 +334,19 @@ function LieGroups.get_coordinates_lie!( c[10] = X.x[2].x[2][] # Δt return c end + +function LieGroups.jacobian_exp!( + ::LieGroups.SpecialGalileanGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, + J::AbstractMatrix, + X::ArrayPartition, + ::DefaultLieAlgebraOrthogonalBasis, + ) + Ω = X.x[1].x[1] + ν = X.x[1].x[2] + ρ = X.x[2].x[1] + ι = X.x[2].x[2][] + ω = [Ω[3, 2], Ω[1, 3], Ω[2, 1]] + # jacobian_exp is the left-trivialized differential of exp (the right Jacobian), + # obtained from the left Jacobian of [Kelly:2025, eq. (31)] as J_r(ξ) = J_ℓ(-ξ) + return LieGroups._jacobian_exp_left_SGal3!(J, -ρ, -ν, -ω, -ι) +end diff --git a/ext/LieGroupsTestExt.jl b/ext/LieGroupsTestExt.jl index e9cd2016..d6284bab 100644 --- a/ext/LieGroupsTestExt.jl +++ b/ext/LieGroupsTestExt.jl @@ -892,7 +892,7 @@ end """ LieGroups.Test.test_jacobian_exp( - G::AbstractLieGroup, g, X; + G::AbstractLieGroup, X; basis = DefaultLieAlgebraOrthogonalBasis(), expected = missing, test_mutating = true, @@ -903,19 +903,19 @@ test `jacobian_exp`. """ function LieGroups.Test.test_jacobian_exp( - G::AbstractLieGroup, g, X; + G::AbstractLieGroup, X; basis = DefaultLieAlgebraOrthogonalBasis(), expected = missing, test_mutating::Bool = true, kwargs..., ) @testset "Jacobian of the exponential map with respect to its argument" begin - J = jacobian_exp(G, g, X, basis) + J = jacobian_exp(G, X, basis) n = manifold_dimension(base_manifold(G)) @test size(J) == (n, n) if test_mutating J2 = copy(J) - jacobian_exp!(G, J2, g, X, basis) + jacobian_exp!(G, J2, X, basis) @test isapprox(J, J2; kwargs...) end !ismissing(expected) && (@test isapprox(J, expected; kwargs...)) @@ -1343,7 +1343,7 @@ function LieGroups.Test.test_lie_group(G::AbstractLieGroup, properties::Dict, ex end if (jacobian_exp in functions) expected = get(expectations, :jacobian_exp, missing) - LieGroups.Test.test_jacobian_exp(G, points[1], vectors[1]; expected = expected) + LieGroups.Test.test_jacobian_exp(G, vectors[1]; expected = expected) end # # diff --git a/src/group_operations/addition_operation.jl b/src/group_operations/addition_operation.jl index bcdcef4d..246c7e10 100644 --- a/src/group_operations/addition_operation.jl +++ b/src/group_operations/addition_operation.jl @@ -21,6 +21,12 @@ Base.:-(e::Identity{AdditionGroupOperation}, ::Identity{AdditionGroupOperation}) Base.:-(::Identity{AdditionGroupOperation}, g) = -g Base.:-(g, ::Identity{AdditionGroupOperation}) = g +# Identity{AdditionGroupOperation} is a valid point on a Lie algebra and the same as the zero vector. +Base.:+(e::Identity{AdditionGroupOperation}, ::ManifoldsBase.ZeroVector) = e +Base.:+(::ManifoldsBase.ZeroVector, e::Identity{AdditionGroupOperation}) = e +Base.:-(e::Identity{AdditionGroupOperation}, ::ManifoldsBase.ZeroVector) = e +Base.:-(::ManifoldsBase.ZeroVector, e::Identity{AdditionGroupOperation}) = e + _doc_compose_add = """ compose(G::LieGroup{𝔽,AdditionGroupOperation}, g, h) compose!(G::LieGroup{𝔽,AdditionGroupOperation}, k, g, h) @@ -142,6 +148,28 @@ end return convert(T, c) end +_doc_jacobian_exp_add = """ + jacobian_exp(G::LieGroup{𝔽,AdditionGroupOperation}, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(G::LieGroup{𝔽,AdditionGroupOperation}, J, X, ::DefaultLieAlgebraOrthogonalBasis) + +Compute the Jacobian of the Lie group exponential in a basis of the Lie algebra on a +[`LieGroup`](@ref) with an [`AdditionGroupOperation`](@ref). + +Since such a group is Abelian and flat, the differential of the exponential map is the +identity, so ``J = I_n`` for every ``X``. +""" + +@doc "$(_doc_jacobian_exp_add)" +jacobian_exp(::LieGroup{𝔽, AdditionGroupOperation}, X, basis = DefaultLieAlgebraOrthogonalBasis()) where {𝔽} + +@doc "$(_doc_jacobian_exp_add)" +function jacobian_exp!( + ::LieGroup{𝔽, AdditionGroupOperation}, J::AbstractMatrix, X, ::DefaultLieAlgebraOrthogonalBasis + ) where {𝔽} + copyto!(J, LinearAlgebra.I) + return J +end + _doc_identity_element_add = """ identity_element(G::LieGroup{𝔽,AdditionGroupOperation}) identity_element!(G::LieGroup{𝔽,AdditionGroupOperation}, e) diff --git a/src/groups/product_group.jl b/src/groups/product_group.jl index 190ac158..40bd5329 100644 --- a/src/groups/product_group.jl +++ b/src/groups/product_group.jl @@ -297,6 +297,46 @@ function ManifoldsBase.exp!( return h end +_doc_jacobian_exp_product = raw""" + jacobian_exp(G::ProductLieGroup, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(G::ProductLieGroup, J, X, ::DefaultLieAlgebraOrthogonalBasis) + +Compute the Jacobian of the Lie group exponential in a basis of the Lie algebra on a +[`ProductLieGroup`](@ref). + +Since on a (direct) product ``G = G_1 × ⋯ × G_n`` the exponential map acts componentwise +and the basis of the [`LieAlgebra`](@ref) is the concatenation of the bases of the factors, +the Jacobian is block-diagonal with the [`jacobian_exp`](@ref) of the factors on its +diagonal, + +````math +J = \begin{pmatrix} J_1 & & \\ & \ddots & \\ & & J_n \end{pmatrix}, +\qquad J_i = \text{jacobian\_exp}(G_i, X_i). +```` +""" + +@doc "$(_doc_jacobian_exp_product)" +jacobian_exp( + ::LieGroup{𝔽, <:ProductGroupOperation, <:ProductManifold}, X, ::AbstractBasis +) where {𝔽} + +@doc "$(_doc_jacobian_exp_product)" +function jacobian_exp!( + PrG::LieGroup{𝔽, Op, M}, J, X, B::DefaultLieAlgebraOrthogonalBasis + ) where {𝔽, Op <: ProductGroupOperation, M <: ProductManifold} + PrM = PrG.manifold + dims = map(manifold_dimension, PrM.manifolds) + dim_ranges = ManifoldsBase._get_dim_ranges(dims) + fill!(J, 0) + foreach( + (Gi, Xi, dr) -> jacobian_exp!(Gi, view(J, dr, dr), Xi, B), + map(LieGroup, PrM.manifolds, PrG.op.operations), + submanifold_components(PrM, X), + dim_ranges, + ) + return J +end + function get_vector_lie!( Pr𝔤::LieAlgebra{𝔽, Op, LieGroup{𝔽, Op, M}}, X, c, B::DefaultLieAlgebraOrthogonalBasis ) where {𝔽, Op <: AbstractProductGroupOperation, M <: ProductManifold} diff --git a/src/groups/special_euclidean_group.jl b/src/groups/special_euclidean_group.jl index 63c8f0e9..95839143 100644 --- a/src/groups/special_euclidean_group.jl +++ b/src/groups/special_euclidean_group.jl @@ -817,6 +817,200 @@ function _log_SE3!(G::SpecialEuclideanGroup{ManifoldsBase.TypeParameter{Tuple{3} return X end +# rotation coordinates come first in the Lie algebra basis of the left semidirect +# variant SO(n)⋉T(n), translation coordinates first for the right variant T(n)⋊SO(n) +_se_translation_first(::LeftSpecialEuclideanGroup) = false +_se_translation_first(::RightSpecialEuclideanGroup) = true + +_doc_jacobian_exp_SE2 = """ + jacobian_exp(G::SpecialEuclideanGroup{TypeParameter{Tuple{2}}}, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(G::SpecialEuclideanGroup{TypeParameter{Tuple{2}}}, J, X, ::DefaultLieAlgebraOrthogonalBasis) + +Compute the Jacobian of the Lie group exponential in a basis of the Lie algebra on the +[`SpecialEuclideanGroup`](@ref)`(2)`. + +For ``X ∈ $(_math(:se))(2)`` with translation component ``v = (x, y)`` and rotation angle ``θ``, +the closed form follows from summing the series +``J = $(_tex(:sum))_{k ≥ 0} $(_tex(:frac, "(-$(_tex(:rm, "ad"))_X)^k", "(k+1)!"))`` block-wise +(compare [Chirikjian:2012](@cite) and [SolaDerayAtchuthan:2021](@cite), Appendix A). +In the coordinate order ``(x, y, θ)`` of the right variant ``$(_math(:T))(2) ⋊ $(_math(:SO))(2)`` +it reads + +```math +J = $( + _tex( + :pmatrix, + "$(_tex(:frac, "$(_tex(:sin)) θ", "θ")) & $(_tex(:frac, "1-$(_tex(:cos)) θ", "θ")) & W_2 x - W_1 y", + "-$(_tex(:frac, "1-$(_tex(:cos)) θ", "θ")) & $(_tex(:frac, "$(_tex(:sin)) θ", "θ")) & W_1 x + W_2 y", + "0 & 0 & 1", + ) +), +$(_tex(:qquad)) +W_1 = $(_tex(:frac, "1-$(_tex(:cos)) θ", "θ^2")),$(_tex(:quad)) W_2 = $(_tex(:frac, "θ-$(_tex(:sin)) θ", "θ^2")), +``` + +with the corresponding Taylor expansions used near ``θ = 0``. For the left variant +``$(_math(:SO))(2) ⋉ $(_math(:T))(2)`` the rows and columns are permuted to the coordinate +order ``(θ, x, y)``. +""" + +@doc "$(_doc_jacobian_exp_SE2)" +jacobian_exp(::SpecialEuclideanGroup{ManifoldsBase.TypeParameter{Tuple{2}}}, X, basis = DefaultLieAlgebraOrthogonalBasis()) + +@doc "$(_doc_jacobian_exp_SE2)" +function jacobian_exp!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{2}}}, + J::AbstractMatrix, X::AbstractMatrix, ::DefaultLieAlgebraOrthogonalBasis, + ) + return _jacobian_exp_SE2!(G, J, X) +end +function jacobian_exp!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{2}}}, + J::AbstractMatrix, + X::SpecialEuclideanMatrixTangentVector, + B::DefaultLieAlgebraOrthogonalBasis, + ) + return jacobian_exp!(G, J, ManifoldsBase.internal_value(X), B) +end + +function _jacobian_exp_SE2!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{2}}}, J, X + ) + Y = submanifold_component(G, X, :Rotation) + v = submanifold_component(G, X, :Translation) + θ = Y[2, 1] + x, y = v[1], v[2] + if abs(θ) < 1.0e-4 # Taylor expansions, numerically robust near θ = 0 + A = 1 - θ^2 / 6 + B = θ / 2 - θ^3 / 24 + W₁ = 1 / 2 - θ^2 / 24 + W₂ = θ / 6 - θ^3 / 120 + else + sθ, cθ = sincos(θ) + A = sθ / θ + B = (1 - cθ) / θ + W₁ = (1 - cθ) / θ^2 + W₂ = (θ - sθ) / θ^2 + end + q₁ = W₂ * x - W₁ * y + q₂ = W₁ * x + W₂ * y + fill!(J, 0) + if _se_translation_first(G) # coordinate order (x, y, θ) + J[1, 1] = A + J[1, 2] = B + J[1, 3] = q₁ + J[2, 1] = -B + J[2, 2] = A + J[2, 3] = q₂ + J[3, 3] = 1 + else # coordinate order (θ, x, y) + J[1, 1] = 1 + J[2, 1] = q₁ + J[2, 2] = A + J[2, 3] = B + J[3, 1] = q₂ + J[3, 2] = -B + J[3, 3] = A + end + return J +end + +_doc_jacobian_exp_SE3 = """ + jacobian_exp(G::SpecialEuclideanGroup{TypeParameter{Tuple{3}}}, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(G::SpecialEuclideanGroup{TypeParameter{Tuple{3}}}, J, X, ::DefaultLieAlgebraOrthogonalBasis) + +Compute the Jacobian of the Lie group exponential in a basis of the Lie algebra on the +[`SpecialEuclideanGroup`](@ref)`(3)`. + +For ``X = (Y, v) ∈ $(_math(:se))(3)`` with rotation component ``Y`` and translation component ``v``, +the Jacobian has the block structure (coordinate order ``(v, ω)`` of the right variant +``$(_math(:T))(3) ⋊ $(_math(:SO))(3)``, ``ω = Y^{\\vee}``) + +```math +J = $(_tex(:pmatrix, "J_{$(_math(:SO))(3)}(Y) & Q_r(v, ω)", "0 & J_{$(_math(:SO))(3)}(Y)")), +``` + +where ``J_{$(_math(:SO))(3)}`` is the [`jacobian_exp`](@ref) on ``$(_math(:SO))(3)`` and the +coupling block is ``Q_r(v, ω) = Q(-v, -ω)`` with the ``Q``-matrix of the left Jacobian +from [Chirikjian:2012](@cite) (see also [SolaDerayAtchuthan:2021](@cite) and +[Kelly:2025; equation (35)](@cite)). For the left variant ``$(_math(:SO))(3) ⋉ $(_math(:T))(3)`` +the rows and columns are permuted to the coordinate order ``(ω, v)``, moving ``Q_r`` to +the lower left block. +""" + +@doc "$(_doc_jacobian_exp_SE3)" +jacobian_exp(::SpecialEuclideanGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, X, basis = DefaultLieAlgebraOrthogonalBasis()) + +@doc "$(_doc_jacobian_exp_SE3)" +function jacobian_exp!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{3}}}, + J::AbstractMatrix, + X::AbstractMatrix, + ::DefaultLieAlgebraOrthogonalBasis, + ) + return _jacobian_exp_SE3!(G, J, X) +end +function jacobian_exp!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{3}}}, + J::AbstractMatrix, + X::SpecialEuclideanMatrixTangentVector, + B::DefaultLieAlgebraOrthogonalBasis, + ) + return jacobian_exp!(G, J, ManifoldsBase.internal_value(X), B) +end + +function _jacobian_exp_SE3!( + G::SpecialEuclideanGroup{<:ManifoldsBase.TypeParameter{Tuple{3}}}, J, X + ) + Y = submanifold_component(G, X, :Rotation) + v = submanifold_component(G, X, :Translation) + ω₁, ω₂, ω₃ = Y[3, 2], Y[1, 3], Y[2, 1] + θ = sqrt(ω₁^2 + ω₂^2 + ω₃^2) + if θ < 1.0e-4 # Taylor expansions, numerically robust near θ = 0 + a = -1 / 2 + θ^2 / 24 + b = 1 / 6 - θ^2 / 120 + else + sθ, cθ = sincos(θ) + a = (cθ - 1) / θ^2 + b = (θ - sθ) / θ^3 + end + R = LinearAlgebra.I + a .* Y .+ b .* (Y * Y) # jacobian_exp on SO(3) + # right Jacobian coupling block is the left Jacobian Q-matrix evaluated at -X + Q = _jacobian_exp_SE3_Q(-v[1], -v[2], -v[3], -ω₁, -ω₂, -ω₃) + fill!(J, 0) + if _se_translation_first(G) # coordinate order (v, ω) + J[1:3, 1:3] .= R + J[1:3, 4:6] .= Q + J[4:6, 4:6] .= R + else # coordinate order (ω, v) + J[1:3, 1:3] .= R + J[4:6, 1:3] .= Q + J[4:6, 4:6] .= R + end + return J +end + +# The Q-matrix coupling translation and rotation of the left Jacobian on SE(3), +# see [Chirikjian:2012] and [SolaDerayAtchuthan:2021], in the coordinate order (ρ, φ) +function _jacobian_exp_SE3_Q(ρ₁, ρ₂, ρ₃, φ₁, φ₂, φ₃) + θ = sqrt(φ₁^2 + φ₂^2 + φ₃^2) + ρx = [0 -ρ₃ ρ₂; ρ₃ 0 -ρ₁; -ρ₂ ρ₁ 0.0] + φx = [0 -φ₃ φ₂; φ₃ 0 -φ₁; -φ₂ φ₁ 0.0] + if θ < 1.0e-4 # Taylor expansions, numerically robust near θ = 0 + c₁ = 1 / 6 - θ^2 / 120 + c₂ = 1 / 24 - θ^2 / 720 + c₃ = (c₂ - 3 * (-1 / 120 + θ^2 / 5040)) / 2 + else + sθ, cθ = sincos(θ) + c₁ = (θ - sθ) / θ^3 + c₂ = (1 - θ^2 / 2 - cθ) / θ^4 + c₃ = (c₂ - 3 * (θ - sθ - θ^3 / 6) / θ^5) / 2 + end + return ρx ./ 2 .+ c₁ .* (φx * ρx + ρx * φx + φx * ρx * φx) .- + c₂ .* (φx * φx * ρx + ρx * φx * φx - 3 .* (φx * ρx * φx)) .- + c₃ .* (φx * ρx * φx * φx + φx * φx * ρx * φx) +end + function LinearAlgebra.norm( 𝔤::LieAlgebra{ℝ, <:SpecialEuclideanGroupOperation, <:SpecialEuclideanGroup}, X::AbstractMatrix, diff --git a/src/groups/special_galilean_group.jl b/src/groups/special_galilean_group.jl index 118a8fdc..af925c7a 100644 --- a/src/groups/special_galilean_group.jl +++ b/src/groups/special_galilean_group.jl @@ -233,3 +233,126 @@ ManifoldsBase.vee( ManifoldsBase.vee!( ::typeof(LieAlgebra(SpecialGalileanGroup(3))), c, X ) + +_doc_jacobian_exp_SGal3 = raw""" + jacobian_exp(G::SpecialGalileanGroup, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(G::SpecialGalileanGroup, J, X, ::DefaultLieAlgebraOrthogonalBasis) + +Compute the Jacobian of the Lie group exponential in a basis of the Lie algebra on the +[`SpecialGalileanGroup`](@ref)`(3)`. + +The closed form of the left Jacobian ``\mathbf{J}_ℓ`` from +[Kelly:2025; section 8, equations (31)–(36)](@cite) is used. In the coordinate order +``ξ = (ρ, ν, ϕ, ι)`` (see [`hat`](@ref)) it has the block structure + +````math +\mathbf{J}_ℓ(ξ) = \begin{pmatrix} +\mathbf{D} & -\mathbf{L}ι & \mathbf{N} & \mathbf{E}ν \\ +\mathbf{0} & \mathbf{D} & \mathbf{M} & \mathbf{0} \\ +\mathbf{0} & \mathbf{0} & \mathbf{D} & \mathbf{0} \\ +\mathbf{0} & \mathbf{0} & \mathbf{0} & 1 +\end{pmatrix} ∈ ℝ^{10×10}, +```` + +where ``\mathbf{D}`` is the left Jacobian of ``\mathrm{SO}(3)``, ``\mathbf{E}`` and +``\mathbf{L}`` are given by [Kelly:2025; equations (19) and (32)](@cite), +``\mathbf{M}`` and ``\mathbf{N} = \mathbf{N}_1 - \mathbf{N}_2`` by +[Kelly:2025; equations (33)–(36)](@cite). Consistent with the convention used for +[`jacobian_exp`](@ref) on the other groups (the left-trivialized differential of the +exponential), this function returns the right Jacobian ``\mathbf{J}_r(ξ) = \mathbf{J}_ℓ(-ξ)``. + +For small rotation angles the Jacobian is evaluated by truncating the series +``\mathbf{J}_ℓ(ξ) = \sum_{n ≥ 0} \frac{1}{(n+1)!} \operatorname{ad}_ξ^n`` of the adjoint +matrix [Kelly:2025; equation (28)](@cite), which is numerically robust there. +""" + +@doc "$(_doc_jacobian_exp_SGal3)" +jacobian_exp(::SpecialGalileanGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, X, basis = DefaultLieAlgebraOrthogonalBasis()) + +@doc "$(_doc_jacobian_exp_SGal3)" +jacobian_exp!(::SpecialGalileanGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, J, X, basis = DefaultLieAlgebraOrthogonalBasis()) + +# Internal function to compute the skew-symmetric matrix as an SMatrix used for performance. +# Can be replaced with hat(SO(3), v) once that works without allocations. +function _skew(v::AbstractVector{T}) where {T <: Real} + return SMatrix{3, 3, T}(0, v[3], -v[2], -v[3], 0, v[1], v[2], -v[1], 0) +end + +# left Jacobian of SGal(3), [Kelly:2025, eqs. (31)-(36)], coordinate order (ρ, ν, ϕ, ι); +# the blocks M and N₁ share one closed form with different arguments, see _sgal3_MN₁ +function _jacobian_exp_left_SGal3!(J::AbstractMatrix, ρ, ν, ω, ι) + φ = sqrt(ω[1]^2 + ω[2]^2 + ω[3]^2) + fill!(J, 0) + if φ < 0.15 + # Truncated series of the adjoint matrix [Kelly:2025, eq. (28)] to avoid the + # catastrophic cancellation of the closed form below, whose N₂ block (eq. (36)) + # divides by φ³ and loses ~3 extra digits per decade as φ → 0. The switch point + # φ = 0.15 is where the closed form stops being the more accurate branch; + # so 9 terms reach ~1e-14 at φ = 0.15. Both were chosen by measuring each + # branch against a BigFloat ground truth. + mad = zeros(MMatrix{10, 10, Float64}) + Ωx = _skew(ω) + mad[1:3, 1:3] .= Ωx + mad[4:6, 4:6] .= Ωx + mad[7:9, 7:9] .= Ωx + for i in 1:3 + mad[i, 3 + i] = -ι + end + mad[1:3, 7:9] .= _skew(ρ) + mad[1:3, 10] .= ν + mad[4:6, 7:9] .= _skew(ν) + ad = SMatrix(mad) + term = SMatrix{10, 10, Float64}(LinearAlgebra.I) + Js = term + for k in 1:9 + term = term * ad ./ (k + 1) # tₖ = adᵏ/(k+1)! + Js += term + end + J .= Js + return J + end + u = ω ./ φ + ux = _skew(u) + ux² = ux * ux + νx = _skew(ν) + sφ, cφ = sincos(φ) + I₃ = Matrix{Float64}(LinearAlgebra.I, 3, 3) + # [Kelly:2025, eq. (18)] + D = I₃ .+ ((1 - cφ) / φ) .* ux .+ ((φ - sφ) / φ) .* ux² + # [Kelly:2025, eq. (19)] + E = I₃ ./ 2 .+ ((φ - sφ) / φ^2) .* ux .+ ((φ^2 + 2cφ - 2) / (2φ^2)) .* ux² + # [Kelly:2025, eq. (32)] + L = I₃ ./ 2 .+ ((sφ - φ * cφ) / φ^2) .* ux .+ + ((φ^2 + 2 - 2φ * sφ - 2cφ) / (2φ^2)) .* ux² + # [Kelly:2025, eqs. (33) and (35)] + M = _sgal3_MN₁(νx, ux, ux², φ, sφ, cφ) + N₁ = _sgal3_MN₁(_skew(ρ), ux, ux², φ, sφ, cφ) + # [Kelly:2025, eq. (36)] + N₂ = ( + ((2 - φ * sφ - 2cφ) / φ^3) .* ux .+ ((φ + φ * cφ - 2sφ) / φ^3) .* ux² + ) * νx .* ι .+ + ((4sφ - φ^2 * sφ - 4φ * cφ) / (2φ^3)) .* (ux * νx * ux) .* ι .+ + ((4 + φ^2 + φ^2 * cφ - 4φ * sφ - 4cφ) / (2φ^3)) .* (ux² * νx * ux) .* ι .+ + νx * ( + ((φ^2 + 2cφ - 2) / (2φ^3)) .* ux .+ ((sφ - φ) / φ^3) .* ux² + ) .* ι + N = N₁ .- N₂ + J[1:3, 1:3] .= D + J[1:3, 4:6] .= (-ι) .* L + J[1:3, 7:9] .= N + J[1:3, 10] .= E * ν + J[4:6, 4:6] .= D + J[4:6, 7:9] .= M + J[7:9, 7:9] .= D + J[10, 10] = 1 + return J +end + +# shared closed form of the blocks M [Kelly:2025, eq. (33)] and N₁ [eq. (35)], with +# wx the skew matrix of ν and ρ, respectively +function _sgal3_MN₁(wx, ux, ux², φ, sφ, cφ) + return ((1 - cφ) / φ^2) .* wx .+ + ((φ - sφ) / φ^2) .* (ux * wx .+ wx * ux) .+ + ((2 - φ * sφ - 2cφ) / φ^2) .* (ux * wx * ux) .+ + ((2φ + φ * cφ - 3sφ) / φ^2) .* (ux * wx * ux²) +end diff --git a/src/groups/special_orthogonal_group.jl b/src/groups/special_orthogonal_group.jl index fc52de3a..f0597d53 100644 --- a/src/groups/special_orthogonal_group.jl +++ b/src/groups/special_orthogonal_group.jl @@ -82,8 +82,8 @@ _inv!(G::SpecialOrthogonalGroup, k, g) = copyto!(G, k, transpose(g)) _doc_jacobian_exp_SO2 = """ - jacobian_exp(M::SpecialOrthogonalGroup{TypeParameter{Tuple{2}}}, g, X, ::DefaultLieAlgebraOrthogonalBasis) - jacobian_exp!(M::SpecialOrthogonalGroup{TypeParameter{Tuple{2}}}, J, g, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp(M::SpecialOrthogonalGroup{TypeParameter{Tuple{2}}}, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(M::SpecialOrthogonalGroup{TypeParameter{Tuple{2}}}, J, X, ::DefaultLieAlgebraOrthogonalBasis) Compute Jacobian of the Lie group exponential in a basis of the Lie algebra on the [`SpecialOrthogonalGroup`](@ref)`(2)` manifold. @@ -91,19 +91,19 @@ It is equal to matrix ``[1]``, see [SolaDerayAtchuthan:2021](@cite), Appendix A. """ @doc "$(_doc_jacobian_exp_SO2)" -jacobian_exp(::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{2}}}, p, X, basis = DefaultLieAlgebraOrthogonalBasis()) +jacobian_exp(::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{2}}}, X, basis = DefaultLieAlgebraOrthogonalBasis()) @doc "$(_doc_jacobian_exp_SO2)" function jacobian_exp!( - ::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{2}}}, J::AbstractMatrix, p, X, ::DefaultLieAlgebraOrthogonalBasis + ::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{2}}}, J::AbstractMatrix, X, ::DefaultLieAlgebraOrthogonalBasis ) J .= 1 return J end _doc_jacobian_exp_SO3 = raw""" - jacobian_exp(M::SpecialOrthogonalGroup{TypeParameter{Tuple{3}}}, p, X, ::DefaultLieAlgebraOrthogonalBasis) - jacobian_exp!(M::SpecialOrthogonalGroup{TypeParameter{Tuple{3}}}, J, p, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp(M::SpecialOrthogonalGroup{TypeParameter{Tuple{3}}}, X, ::DefaultLieAlgebraOrthogonalBasis) + jacobian_exp!(M::SpecialOrthogonalGroup{TypeParameter{Tuple{3}}}, J, X, ::DefaultLieAlgebraOrthogonalBasis) Compute Jacobian of the Lie group exponential in a basis of the Lie algebra on the [`SpecialOrthogonalGroup`](@ref)`(3)` manifold. The formula reads @@ -116,13 +116,11 @@ It is adapted from [Chirikjian:2012](@cite), Eq. (10.86), to `LieGroups.jl` conv """ @doc "$(_doc_jacobian_exp_SO3)" -jacobian_exp(M::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, p, X, basis = DefaultLieAlgebraOrthogonalBasis()) +jacobian_exp(M::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, X, basis = DefaultLieAlgebraOrthogonalBasis()) @doc "$(_doc_jacobian_exp_SO3)" -function jacobian_exp!( - M::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, J::AbstractMatrix, p, X, ::DefaultLieAlgebraOrthogonalBasis - ) - θ = norm(M, p, X) / sqrt(2) +function jacobian_exp!(M::SpecialOrthogonalGroup{ManifoldsBase.TypeParameter{Tuple{3}}}, J::AbstractMatrix, X, ::DefaultLieAlgebraOrthogonalBasis) + θ = norm(LieAlgebra(M), X) / sqrt(2) copyto!(J, I) if θ ≉ 0 a = (cos(θ) - 1) / θ^2 diff --git a/src/interface.jl b/src/interface.jl index e849f3f3..ba3b62bc 100644 --- a/src/interface.jl +++ b/src/interface.jl @@ -888,28 +888,63 @@ function jacobian_conjugate!( end _doc_jac_exp = """ - jacobian_exp(G::AbstractLieGroup, g, X, b) - jacobian_exp!(G::AbstractLieGroup, J, g, X, b) + jacobian_exp(G::AbstractLieGroup, X, b) + jacobian_exp!(G::AbstractLieGroup, J, X, b) -Compute the Jacobian of the [`exp`](@ref) ``$(_tex(:exp))_g(X)`` with respect to -an [`AbstractBasis`](@extref `ManifoldsBase.AbstractBasis`) of the [`LieAlgebra`](@ref). +Compute the Jacobian of the [Lie group exponential function](@ref exp(::AbstractLieGroup, ::Identity, :Any)) +``$(_tex(:exp))_{$(_math(:G))}: $(_math(:𝔤)) → $(_math(:G))`` at ``X ∈ $(_math(:𝔤))``, +represented in an [`AbstractBasis`](@extref `ManifoldsBase.AbstractBasis`) ``b`` of the [`LieAlgebra`](@ref) ``$(_math(:𝔤))``. + +The (classical) differential ``$(_math(:D))$(_tex(:exp))_{$(_math(:G))}(X): $(_math(:𝔤)) → T_{$(_tex(:exp))_{$(_math(:G))}(X)}$(_math(:G))`` +maps a tangent vector of the Lie algebra to a tangent vector at the point ``$(_tex(:exp))_{$(_math(:G))}(X)``. +To turn this into a map ``$(_math(:𝔤)) → $(_math(:𝔤))``, that is representable as a matrix in a basis of ``$(_math(:𝔤))``, +we _left-trivialize_ it: analogous to [`diff_right_compose`](@ref) we “pull back” the resulting tangent vector +by multiplying with ``$(_tex(:exp))_{$(_math(:G))}(X)^{-1}`` from the left. +The resulting left-trivialized differential ``$(_math(:d))$(_tex(:exp))_{$(_math(:G))}(X): $(_math(:𝔤)) → $(_math(:𝔤))`` +has the series representation + +```math +$(_math(:d))$(_tex(:exp))_{$(_math(:G))}(X) = $(_tex(:sum))_{k ≥ 0} $(_tex(:frac, "(-$(_tex(:rm, "ad"))_X)^k", "(k+1)!")), +``` + +where ``$(_tex(:rm, "ad"))_X = [X, ⋅]`` denotes the adjoint of the [`LieAlgebra`](@ref), see [`lie_bracket`](@ref). +The Jacobian ``J`` is the matrix of this map with respect to the basis ``b``: its ``j``th column contains the +coordinates of ``$(_math(:d))$(_tex(:exp))_{$(_math(:G))}(X)[X_j]``, where ``X_j`` is the ``j``th basis vector of ``b``. +Since it only depends on ``X``, this Jacobian is independent of a base point, which is why no point is passed. + +!!! note + In the robotics and state-estimation literature this left-trivialized differential is often called the + _right Jacobian_ ``J_r``, for example in [SolaDerayAtchuthan:2021](@cite) and [Chirikjian:2012](@cite). + It is related to the _left Jacobian_ ``J_ℓ`` by ``J_r(X) = J_ℓ(-X)``, which is the right-trivialized + differential ``$(_tex(:sum))_{k ≥ 0} $(_tex(:frac, "($(_tex(:rm, "ad"))_X)^k", "(k+1)!"))``. """ "$(_doc_jac_exp)" function jacobian_exp( - G::AbstractLieGroup, g, X, B::AbstractBasis = DefaultLieAlgebraOrthogonalBasis() + G::AbstractLieGroup, X, B::AbstractBasis = DefaultLieAlgebraOrthogonalBasis() ) - J = ManifoldsBase.allocate_result(G, jacobian_exp, g, X, B) - return jacobian_exp!(G, J, g, X, B) + J = ManifoldsBase.allocate_result(G, jacobian_exp, X, B) + return jacobian_exp!(G, J, X, B) end +#TODO deprecated in v0.1.12, remove in v0.2.0 +@deprecate jacobian_exp( + G::AbstractLieGroup, g, X, B::AbstractBasis = DefaultLieAlgebraOrthogonalBasis() +) jacobian_exp(G, X, B) + function jacobian_exp! end @doc "$(_doc_jac_exp)" jacobian_exp!( - G::AbstractLieGroup, J, g, X, + G::AbstractLieGroup, J, X, B::AbstractBasis = DefaultLieAlgebraOrthogonalBasis() ) +#TODO deprecated in v0.1.12, remove in v0.2.0 +@deprecate jacobian_exp!( + G::AbstractLieGroup, J, g, X, + B::AbstractBasis = DefaultLieAlgebraOrthogonalBasis() +) jacobian_exp!(G, J, X, B) + _doc_log = """ log(G::AbstractLieGroup, g, h) log!(G::AbstractLieGroup, X, g, h) @@ -1269,9 +1304,9 @@ function ManifoldsBase.allocate_result(G::LieGroup, f::typeof(jacobian_conjugate n = number_of_coordinates(G.manifold, B) return zeros(float(number_eltype(g)), n, n) end -function ManifoldsBase.allocate_result(G::LieGroup, f::typeof(jacobian_exp), g, X, B) +function ManifoldsBase.allocate_result(G::LieGroup, f::typeof(jacobian_exp), X, B) n = number_of_coordinates(G.manifold, B) - return zeros(float(number_eltype(g)), n, n) + return zeros(float(number_eltype(X)), n, n) end function ManifoldsBase.allocate_result(G::AbstractLieGroup, f::typeof(log), args...) return ManifoldsBase.allocate_result(base_manifold(G), f, args...) diff --git a/test/groups/jacobian_exp_series_reference.jl b/test/groups/jacobian_exp_series_reference.jl new file mode 100644 index 00000000..6e5e1928 --- /dev/null +++ b/test/groups/jacobian_exp_series_reference.jl @@ -0,0 +1,40 @@ +# Independent series ground truth for `jacobian_exp`, built purely from LieGroups +# primitives (`lie_bracket`, `hat`, `vee`) so the tests validate the closed forms +# against math rather than hard-coded magic numbers. Shared by the SE and SGal tests. + +using LieGroups, ManifoldsBase +using LinearAlgebra + +if !isdefined(@__MODULE__, :_jacobian_exp_series) + + # small adjoint matrix on the lie algebra X (ad_X), assembled column by column from the lie_bracket, + # ad_X e_j = [X, E_j]. The input is re-`hat`ed from its coordinates so the bracket sees a + # single, representation-neutral tangent type. + function _adjoint_algebra_matrix(G, X) + 𝔤 = LieAlgebra(G) + n = manifold_dimension(G) + Xc = hat(𝔤, vee(𝔤, X)) + A = zeros(n, n) + for j in 1:n + e_j = zeros(n) + e_j[j] = 1.0 + A[:, j] = vee(𝔤, lie_bracket(𝔤, Xc, hat(𝔤, e_j))) + end + return A + end + + # Left-trivialized Jacobian of exp via its series J_exp(X) = ∑_k (-ad_X)^k / (k+1)! + # [Kelly:2025, eq. (28)], an independent check for the closed-form `jacobian_exp` + function _jacobian_exp_series(G, X; order = 20) + ad = _adjoint_algebra_matrix(G, X) + n = size(ad, 1) + J = Matrix{Float64}(LinearAlgebra.I, n, n) + term = copy(J) + for k in 1:order + term = term * (-ad) ./ (k + 1) # tₖ = (-ad)ᵏ / (k+1)! + J .+= term + end + return J + end + +end diff --git a/test/groups/test_product_group.jl b/test/groups/test_product_group.jl index 879b7654..f62add06 100644 --- a/test/groups/test_product_group.jl +++ b/test/groups/test_product_group.jl @@ -1,4 +1,4 @@ -using LieGroups, Test, ManifoldsBase, Random, RecursiveArrayTools +using LieGroups, Test, ManifoldsBase, Random, RecursiveArrayTools, LinearAlgebra @testset "Generic product Lie group" begin G = TranslationGroup(2) × TranslationGroup(2) @@ -23,6 +23,7 @@ using LieGroups, Test, ManifoldsBase, Random, RecursiveArrayTools inv_left_compose, inv_right_compose, is_identity, + jacobian_exp, lie_bracket, log, rand, @@ -33,8 +34,34 @@ using LieGroups, Test, ManifoldsBase, Random, RecursiveArrayTools @test LieGroups.submanifold_components(G, Identity(G)) === (Identity{AdditionGroupOperation}(), Identity{AdditionGroupOperation}()) expectations = Dict( :repr => "ProductLieGroup(Euclidean(2; field=ℝ) × Euclidean(2; field=ℝ), AdditionGroupOperation() × AdditionGroupOperation())", + # both factors are flat and Abelian, so the Jacobian of exp is the identity + :jacobian_exp => Matrix{Float64}(LinearAlgebra.I, 4, 4), ) LieGroups.Test.test_lie_group(G, properties, expectations) + @testset "jacobian_exp is block-diagonal over the factors" begin + B = DefaultLieAlgebraOrthogonalBasis() + # a product with curvature: SO(3) × SE(2) (right variant) + Gc = SpecialOrthogonalGroup(3) × SpecialEuclideanGroup(2; variant = :right) + 𝔤c = LieAlgebra(Gc) + Xcc = [0.3, -0.2, 0.5, 0.7, -0.4, 0.6] + Xt = hat(𝔤c, Xcc) + Jc = jacobian_exp(Gc, Xt) + @test size(Jc) == (6, 6) + # off-diagonal coupling blocks vanish + @test iszero(Jc[1:3, 4:6]) + @test iszero(Jc[4:6, 1:3]) + # diagonal blocks equal each factor's own jacobian_exp + Gc1 = SpecialOrthogonalGroup(3) + Xc1 = hat(LieAlgebra(Gc1), Xcc[1:3]) + @test isapprox(Jc[1:3, 1:3], jacobian_exp(Gc1, Xc1)) + Gc2 = SpecialEuclideanGroup(2; variant = :right) + Xc2 = hat(LieAlgebra(Gc2), Xcc[4:6]) + @test isapprox(Jc[4:6, 4:6], jacobian_exp(Gc2, Xc2)) + # mutating matches allocating + Jc2 = copy(Jc) + jacobian_exp!(Gc, Jc2, Xt, B) + @test isapprox(Jc, Jc2) + end @testset "A small additional size check" begin @test ManifoldsBase.check_size(G, Identity(G)) === nothing @test ManifoldsBase.check_size(G, Identity(G), X) === nothing diff --git a/test/groups/test_special_euclidean_group.jl b/test/groups/test_special_euclidean_group.jl index 2711707b..c7ff5b7f 100644 --- a/test/groups/test_special_euclidean_group.jl +++ b/test/groups/test_special_euclidean_group.jl @@ -2,6 +2,9 @@ using LieGroups, ManifoldsBase, Random, Test, RecursiveArrayTools using Manifolds: Euclidean using StaticArrays +# independent series ground truth for `jacobian_exp` (see the file for details) +include("jacobian_exp_series_reference.jl") + @testset "Special Euclidean" begin fcts = [ compose, @@ -13,6 +16,7 @@ using StaticArrays inv, is_flat, is_identity, + jacobian_exp, lie_bracket, log, norm, @@ -55,7 +59,11 @@ using StaticArrays :Functions => fcts, ) expectations = Dict( - :repr => "SpecialEuclideanGroup(2)", :atol => 1.0e-14, :is_flat => true + :repr => "SpecialEuclideanGroup(2)", :atol => 1.0e-14, :is_flat => true, + # jacobian_exp (on vec[1]) is validated against an independent series + # ground truth built from LieGroups primitives (see the helper at the top + # of this file). + :jacobian_exp => _jacobian_exp_series(G, vec[1]), ) LieGroups.Test.test_lie_group(G, properties, expectations) @test ManifoldsBase.tangent_vector_type(G, typeof(pts[1])) == typeof(vec[1]) @@ -111,7 +119,10 @@ using StaticArrays :Functions => fcts, ) expectations = Dict( - :repr => "SpecialEuclideanGroup(2; variant=:right)", :atol => 1.0e-14 + :repr => "SpecialEuclideanGroup(2; variant=:right)", :atol => 1.0e-14, + # same Lie algebra element as the left variant test above, but in the + # right variant coordinate order; validated against the series ground truth + :jacobian_exp => _jacobian_exp_series(G, vec[1]), ) LieGroups.Test.test_lie_group(G, properties, expectations) end @@ -161,7 +172,11 @@ using StaticArrays :Functions => fcts, ) expectations = Dict( - :repr => "SpecialEuclideanGroup(3)", :atol => 1.0e-14, :is_flat => false + :repr => "SpecialEuclideanGroup(3)", :atol => 1.0e-14, :is_flat => false, + # jacobian_exp (on vec[1]) is validated against an independent series + # ground truth built from LieGroups primitives (see the helper at the top + # of this file) + :jacobian_exp => _jacobian_exp_series(G, vec[1]), ) LieGroups.Test.test_lie_group(G, properties, expectations) @@ -183,6 +198,52 @@ using StaticArrays end end # + # jacobian_exp: exercise BOTH the small-angle Taylor branch (rotation θ < 1e-4) and the + # closed-form branch, each validated against the series ground truth. Both semidirect + # variants are covered so both coordinate orders of the block assembly are hit: the left + # variant SO(n)⋉T(n) (order (ω, v)) and the right variant T(n)⋊SO(n) (translation first, + # order (v, ω)). + @testset "jacobian_exp small- and large-angle branches" begin + for variant in (:left, :right) + for (n, mkX) in ( + (2, θ -> [0.0 -θ 1.0; θ 0.0 0.5; 0.0 0.0 0.0]), + (3, θ -> [0.0 -θ 0.0 1.0; θ 0.0 0.0 0.5; 0.0 0.0 0.0 0.3; 0.0 0.0 0.0 0.0]), + ) + G = SpecialEuclideanGroup(n; variant) + for θ in (1.0e-6, 0.23) # 1e-6 → Taylor branch, 0.23 → closed form + X = mkX(θ) + @test isapprox( + jacobian_exp(G, X), _jacobian_exp_series(G, X); atol = 1.0e-12 + ) + end + end + end + end + + # TODO deprecated in v0.1.12, remove in v0.2.0 + # the old jacobian_exp(G, g, X, b)/jacobian_exp!(G, J, g, X, b) signatures (with the + # unused base point g) are deprecated in favor of jacobian_exp(G, X, b); check both the + # default-basis and explicit-basis deprecated methods still forward to the same result + @testset "jacobian_exp deprecated g argument" begin + G = SpecialEuclideanGroup(2) + g = identity_element(G) + X = [0.0 -0.23 1.0; 0.23 0.0 0.5; 0.0 0.0 0.0] + basis = DefaultLieAlgebraOrthogonalBasis() + J = jacobian_exp(G, X) + + J_old = Test.@test_deprecated jacobian_exp(G, g, X) + @test isapprox(J, J_old) + J_old_basis = Test.@test_deprecated jacobian_exp(G, g, X, basis) + @test isapprox(J, J_old_basis) + + J2 = similar(J) + Test.@test_deprecated jacobian_exp!(G, J2, g, X) + @test isapprox(J, J2) + J3 = similar(J) + Test.@test_deprecated jacobian_exp!(G, J3, g, X, basis) + @test isapprox(J, J3) + end + # # # SE(4) @testset "SE(4)" begin diff --git a/test/groups/test_special_galilean_group.jl b/test/groups/test_special_galilean_group.jl index 38aa5ee8..47206548 100644 --- a/test/groups/test_special_galilean_group.jl +++ b/test/groups/test_special_galilean_group.jl @@ -4,6 +4,22 @@ using LieGroups: SpecialGalileanGroup using StaticArrays using LinearAlgebra +# --- independent ground truths built from LieGroups primitives (no magic numbers) --- + +# small adjoint matrix ad_X and the right-Jacobian series ground truth for `jacobian_exp` +include("jacobian_exp_series_reference.jl") + +# reference Lie bracket via the matrix commutator of the 5×5 screw representation +# (see the `hat` docstring), an independent check for the closed-form `lie_bracket` +_sgal3_skew(a) = [0.0 -a[3] a[2]; a[3] 0.0 -a[1]; -a[2] a[1] 0.0] +_sgal3_screw(c) = [_sgal3_skew(c[7:9]) c[4:6] c[1:3]; zeros(1, 3) 0.0 c[10]; zeros(1, 5)] +_sgal3_coords(Z) = [Z[1:3, 5]; Z[1:3, 4]; Z[3, 2]; Z[1, 3]; Z[2, 1]; Z[4, 5]] +function _lie_bracket_ref(G, X, Y) + 𝔤 = LieAlgebra(G) + Xm, Ym = _sgal3_screw(vee(𝔤, X)), _sgal3_screw(vee(𝔤, Y)) + return hat(𝔤, _sgal3_coords(Xm * Ym - Ym * Xm)) +end + @testset "Special Galilean" begin 𝔰 = sqrt(2) fcts = [ @@ -16,6 +32,8 @@ using LinearAlgebra inv, # is_flat, is_identity, + jacobian_exp, + lie_bracket, log, norm, rand, @@ -69,11 +87,30 @@ using LinearAlgebra :atol => 1.0e-14, # :repr => "SpecialGalileanGroup(3)", # :is_flat => false + # jacobian_exp (on vec[1]) and lie_bracket (on vec[1], vec[2]) are validated + # against independent ground truths built from LieGroups primitives (see the + # helpers at the top of this file) rather than hard-coded magic numbers + :jacobian_exp => _jacobian_exp_series(G, vec[1]), + :lie_bracket => _lie_bracket_ref(G, vec[1], vec[2]), ) LieGroups.Test.test_lie_group(G, properties, expectations) end end + # jacobian_exp: exercise BOTH the truncated-series branch (φ < 0.15) and the + # closed-form branch, each validated against the series ground truth + @testset "jacobian_exp series and closed-form branches" begin + G = SpecialGalileanGroup(3) + mkX = φ -> ArrayPartition( + ArrayPartition([0.0 -φ 0.0; φ 0.0 0.0; 0.0 0.0 0.0], [1.0, 0.5, 0.0]), + ArrayPartition([0.3, 0.0, 0.2], [0.4]), + ) + for φ in (1.0e-3, 0.23) # 1e-3 → series branch (φ<0.15), 0.23 → closed form + X = mkX(φ) + @test isapprox(jacobian_exp(G, X), _jacobian_exp_series(G, X); atol = 1.0e-12) + end + end + @testset "Test SGal(3) SArray" begin G = SpecialGalileanGroup(3) ε = identity_element(G, StaticArray) diff --git a/test/groups/test_translation_group.jl b/test/groups/test_translation_group.jl index 27a282c9..fbed6f14 100644 --- a/test/groups/test_translation_group.jl +++ b/test/groups/test_translation_group.jl @@ -26,6 +26,7 @@ begin inv_right_compose, is_identity, jacobian_conjugate, + jacobian_exp, lie_bracket, log, rand, @@ -38,6 +39,8 @@ begin :diff_inv => -X1, :diff_left_compose => X1, :diff_right_compose => X1, + # flat and Abelian: the Jacobian of exp is the identity + :jacobian_exp => [1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], :lie_bracket => zero(X1), ) LieGroups.Test.test_lie_group(G, properties, expectations) diff --git a/test/operations/test_addition_operation.jl b/test/operations/test_addition_operation.jl index 8cade751..e60cd0aa 100644 --- a/test/operations/test_addition_operation.jl +++ b/test/operations/test_addition_operation.jl @@ -1,5 +1,6 @@ using LieGroups, Test using StaticArrays +using ManifoldsBase @testset "Addition Operation" begin @testset "Base.:+ and Base.:- with the Identity" begin @@ -17,5 +18,10 @@ using StaticArrays @test identity_element(G, Float64) == 0.0 @test identity_element(G, Array{Float64, 0}) == fill(0.0) @test identity_element(G, SArray{Tuple{}, Float64}) == @SArray fill(0.0) + z = ManifoldsBase.ZeroVector() + @test (e + z) === e + @test (z + e) === e + @test (e - z) === e + @test (z - e) === e end end