Update to LieGroups.jl

.gitignore

@@ -1,4 +1,4 @@
-/dev
+dev
 Manifest.toml
 *.so
 *.cov

Project.toml

@@ -18,6 +18,7 @@ ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
 Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
+LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
@@ -55,16 +56,17 @@ Dates = "1.10"
 DelimitedFiles = "1"
 DifferentialEquations = "7"
 Distributed = "1.10"
-DistributedFactorGraphs = "0.25, 0.26"
+DistributedFactorGraphs = "0.27"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"
 Flux = "0.14, 0.15, 0.16"
 ImageCore = "0.9, 0.10"
 ImageIO = "0.6"
-IncrementalInference = "0.35"
+IncrementalInference = "0.36"
 Interpolations = "0.14, 0.15"
 KernelDensityEstimate = "0.5.1, 0.6"
+LieGroups = "0.1.1"
 LinearAlgebra = "1.10"
 Manifolds = "0.10.1"
 ManifoldsBase = "0.15, 1"

src/RoME.jl

@@ -21,7 +21,8 @@ using
   DistributedFactorGraphs,
   TensorCast,
   ManifoldsBase,
-  Manifolds
+  Manifolds,
+  LieGroups
 
 using StaticArrays
 using PrecompileTools
@@ -33,7 +34,7 @@ using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, SpecialOr
 import Manifolds: project, project!, identity_element
 
 import Rotations as _Rot
-import Rotations: ⊕, ⊖ # TODO deprecate
+# import Rotations: ⊕, ⊖ # TODO deprecate
 
 export SpecialOrthogonal, SpecialEuclidean
 export submanifold_component
@@ -65,6 +66,8 @@ include("entities/SpecialDefinitions.jl")
 
 #uses DFG v0.10.2 @defVariable for above
 include("services/FixmeManifolds.jl")
+include("manifolds/SOnxRn_MetricManifold.jl")
+
 include("variables/VariableTypes.jl")
 
 ## More factor types

src/factors/Inertial/IMUDeltaFactor.jl

@@ -394,7 +394,7 @@ function (cf::CalcFactor{<:IMUDeltaFactor})(
     q_SE3,
     q_vel,
     b = zeros(SVector{6,Float64})
-) where T <: Real
+)
     p = ArrayPartition(p_SE3.x[2], p_vel, p_SE3.x[1])
     q = ArrayPartition(q_SE3.x[2], q_vel, q_SE3.x[1])
     return cf(Δmeas, p, q, b)

src/factors/Pose2D.jl

@@ -8,16 +8,15 @@ Rigid transform between two Pose2's, assuming (x,y,theta).
 Calcuated as:
 ```math
 \\begin{aligned}
-\\hat{q}=\\exp_pX_m\\\\
-X = \\log_q \\hat{q}\\\\
-X^i = \\mathrm{vee}(q, X)
+\\hat{X}_p=\\log_pq\\\\
+X^i = \\mathrm{vee}(X_m-\\hat{X})
 \\end{aligned}
 ```
 with:
 ``\\mathcal M= \\mathrm{SE}(2)`` Special Euclidean group\\
 ``p`` and ``q`` ``\\in \\mathcal M`` the two Pose2 points\\
 the measurement vector ``X_m \\in T_p \\mathcal M``\\
-and the error vector ``X \\in T_q \\mathcal M``\\
+and the error vector ``\\hat{X} \\in T_p \\mathcal M``\\
 ``X^i`` coordinates of ``X``
 
 DevNotes
@@ -27,53 +26,28 @@ Related
 
 [`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref)
 """
-Base.@kwdef struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
+Base.@kwdef struct Pose2Pose2{T<:IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
   Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0]))
 end
 
 DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation())
 
 Pose2Pose2(::UniformScaling) = Pose2Pose2()
 
-
-# Assumes X is a tangent vector
-function (cf::CalcFactor{<:Pose2Pose2})(_X::AbstractArray{MT}, _p::AbstractArray{PT}, _q::AbstractArray{LT})  where {MT,PT,LT}
-  #TODO remove this convertions
-  # @warn "This warning should not be triggered after StaticArrays upgrade" maxlog=10
-  T = promote_type(MT, PT, LT)
-  X = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _X)
-  p = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _p)
-  q = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _q)
-  return cf(X,p,q)
-end
-
-# function calcPose2Pose2(
-function (cf::CalcFactor{<:Pose2Pose2})(
-              X::ArrayPartition{XT, Tuple{SVector{2, XT}, SMatrix{2, 2, XT, 4}}},
-              p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, 
-              q::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where {XT<:Real,T<:Real}
-
-    M = getManifold(Pose2)
-    # ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I))
-    ϵ0 = getPointIdentity(M)
-
-    ϵX = exp(M, ϵ0, X)
-    # q̂ = Manifolds.compose(M, p, ϵX)    
-    q̂ = _compose(M, p, ϵX)
-    X_hat = log(M, q, q̂)#::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}
-    # Xc = vee(M, q, X_hat)
-    Xc = _vee(M, X_hat)#::SVector{3,T}
-    return Xc
+function (cf::CalcFactor{<:Pose2Pose2})(X, p, q)
+  # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M
+  M = getManifold(Pose2)
+  X̂ = log(M, p, q)
+  return vee(M, p, X - X̂) # TODO check sign
 end
 
-
 # NOTE, serialization support -- will be reduced to macro in future
 # ------------------------------------
 
 """
 $(TYPEDEF)
 """
-Base.@kwdef struct PackedPose2Pose2  <: AbstractPackedFactor
+Base.@kwdef struct PackedPose2Pose2 <: AbstractPackedFactor
   Z::PackedSamplableBelief
 end
 function convert(::Type{Pose2Pose2}, d::PackedPose2Pose2)
@@ -85,7 +59,7 @@ end
 
 
 # FIXME, rather have separate compareDensity functions
-function compare(a::Pose2Pose2,b::Pose2Pose2; tol::Float64=1e-10)
+function compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64=1e-10)
   return compareDensity(a.Z, b.Z)
 end
 

src/factors/PriorPose2.jl

@@ -16,34 +16,20 @@ end
 
 DFG.getManifold(::InstanceType{PriorPose2}) = getManifold(Pose2) # SpecialEuclidean(2)
 
-@inline function _vee(::typeof(SpecialEuclidean(2; vectors=HybridTangentRepresentation())), X::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real
-  return SVector{3,T}(X.x[1][1],X.x[1][2],X.x[2][2])
-end
-
-@inline function _compose(::typeof(SpecialEuclidean(2; vectors=HybridTangentRepresentation())), p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, q::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real
-  return ArrayPartition(p.x[1] + p.x[2]*q.x[1], p.x[2]*q.x[2])
-end
-
 function (cf::CalcFactor{<:PriorPose2})(_m::AbstractArray{MT}, _p::AbstractArray{PT})  where {MT<:Real,PT<:Real}
   T = promote_type(MT, PT)
   m = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _m)
   p = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _p)
   return cf(m,p)
 end
 
-# TODO the log here looks wrong (for gradients), consider:
-# X = log(p⁻¹ ∘ m) 
-# X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m))
 function (cf::CalcFactor{<:PriorPose2})(
             m::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, 
             p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real
 
   M = getManifold(Pose2)
-  ϵ = getPointIdentity(M)
-  Xc = _vee(M, log(M, p, m))
-  # X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m))
-  # Xc = vee(M, ϵ, X)
-  return Xc
+  X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM?
+  return vee(M, p, X)
 end
 
 #TODO Serialization of reference point p 

src/manifolds/SOnxRn_MetricManifold.jl

@@ -0,0 +1,161 @@
+
+
+# Left Invariant Rigid Body Kinematics Metric CrokeKumar eq 61.
+# A family of left invariant metrics:
+# G = [αI 0; 0 βI]
+# where α and β are arbitrary constants, satisfies all the equations (80). 
+# This are the only left-invariant metrics which are compatible with the acceleration connection.
+# Can we add α and β as parameters to the metric?
+struct LeftInvariantKinematicMetric <: RiemannianMetric end
+
+SOnxRn_MetricManifold(n) = MetricManifold(SpecialEuclideanGroup(n; variant=:right), LeftInvariantKinematicMetric())
+
+SOnxRn_MetricManifoldType = Union{typeof(SOnxRn_MetricManifold(2)), typeof(SOnxRn_MetricManifold(3))}
+
+# geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3
+function Manifolds.exp(M::SOnxRn_MetricManifoldType, p, X)
+    G = base_manifold(M)
+    ε = identity_element(M) #, typeof(p))
+    return LieGroups.compose(G, p, exp(base_manifold(G), ε, X))
+end
+
+function Manifolds.exp!(M::SOnxRn_MetricManifoldType, q, p, X)
+    G = base_manifold(M)
+    ε = identity_element(M) #, typeof(p))
+    return LieGroups.compose!(G, q, p, exp(base_manifold(G), ε, X))
+end
+
+function Manifolds.log(M::SOnxRn_MetricManifoldType, p, q)
+    G = base_manifold(M)
+    ε = identity_element(M) #, typeof(p))
+    X = log(base_manifold(G), ε, LieGroups.compose(G, inv(G, p), q))
+    return X
+end
+
+function Manifolds.log!(M::SOnxRn_MetricManifoldType, X, p, q)
+    G = base_manifold(M)
+    ε = identity_element(M) #, typeof(p))
+    log!(base_manifold(G), X, ε, LieGroups.compose(G, inv(G, p), q))
+    return X
+end
+
+function Manifolds.inner(M::SOnxRn_MetricManifoldType, p, X, Y)
+    Xtr = submanifold_components(M, X)[1]
+    XRo = submanifold_components(M, X)[2]
+    Ytr = submanifold_components(M, Y)[1]
+    YRo = submanifold_components(M, Y)[2]
+    # Metric on Chirikjian, p35 W = diagm([1,1,2])
+    return dot(Xtr,Ytr) + dot(XRo, YRo)/2
+end
+
+function Manifolds.hat(M::SOnxRn_MetricManifoldType, p, X::AbstractVector, T=typeof(p))  
+   return hat(LieAlgebra(base_manifold(M)), X, T)
+end
+
+function ManifoldsBase.get_vector(
+    M::SOnxRn_MetricManifoldType, g, c, B::AbstractBasis{<:Any,TangentSpaceType}
+)
+    G = base_manifold(M)
+    return get_vector(
+        LieAlgebra(G),
+        c,
+        B;
+        tangent_vector_type=ManifoldsBase.tangent_vector_type(G, typeof(g)),
+    )
+end
+
+function ManifoldsBase.get_vector!(
+    M::SOnxRn_MetricManifoldType, X, g, c, B::AbstractBasis{<:Any,TangentSpaceType}
+)
+    G = base_manifold(M)
+    return get_vector!(LieAlgebra(G), X, c, B)
+end
+
+function ManifoldsBase.get_coordinates(
+    M::SOnxRn_MetricManifoldType, g, X, B::AbstractBasis{<:Any,TangentSpaceType}
+)
+    G = base_manifold(M)
+    return get_coordinates(LieAlgebra(G), X, B)
+end
+
+#FIXME: maybe replace this with identity_element
+# call into base_manifold's identity_element, but got ambiguities.
+Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0])
+Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0])
+
+#FIXME remove, only temporary workaround as first signiture is used in many places
+@deprecate LieGroups.identity_element(G::LieGroup, p)  identity_element(G, typeof(p)) false
+
+# FIXME why is this still needed, hopefully can be removed soon 🐛💥
+Base.convert(::Type{<:Tuple}, ::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular)
+
+
+## =======================================================================================
+## SE2 + metric
+## TODO move to test file
+if false
+M = SOnxRn_MetricManifold(2)
+G = base_manifold(M)
+ε = identity_element(M)
+T = typeof(identity_element(M)) 
+Xⁱ = [10, 1, pi/4]
+X = hat(LieAlgebra(G), Xⁱ, T)
+p = exp(base_manifold(G), ε, X)
+q = compose(G, p, exp(base_manifold(G), ε, X))
+
+q ≈ exp(M, p, X)
+X ≈ log(M, p, q)
+
+
+Xⁱ = [1, 1, 1]
+X = hat(LieAlgebra(G), Xⁱ, T)
+p = exp(base_manifold(G), ε, X)
+
+W = diagm([1,1,2])
+
+XX = hat(LieAlgebra(G), Xⁱ)
+0.5*tr(XX*W*XX')
+inner(M, p, X, X)
+inner(base_manifold(G), p, X, X)
+
+Manifolds.distance(M, ε, p)
+Manifolds.distance(base_manifold(G), ε, p)
+
+## SE3 + metric
+
+M = SOnxRn_MetricManifold(3)
+G = base_manifold(M)
+ε = identity_element(M)
+T = typeof(identity_element(M)) 
+Xⁱ = [10, 1, 0, 0, 0, pi/4]
+X = hat(LieAlgebra(G), Xⁱ, T)
+p = exp(base_manifold(G), ε, X)
+q = compose(G, p, exp(base_manifold(G), ε, X))
+
+q ≈ exp(M, p, X)
+X ≈ log(M, p, q)
+
+
+Xⁱ = [0, 0, 0, 0, 1, 1]
+X = hat(LieAlgebra(G), Xⁱ, T)
+p = exp(base_manifold(G), ε, X)
+
+0.5*tr(X.x[2]*X.x[2]')
+dot(X.x[2], X.x[2])
+
+inner(M, p, X, X)
+inner(base_manifold(G), p, X, X)
+
+Manifolds.distance(M, ε, p)
+Manifolds.distance(base_manifold(G), ε, p)
+
+Xⁱ = [1, 1, 0, 0, 1, 1]
+W = diagm([1,1,1,2])
+X = hat(LieAlgebra(G), Xⁱ, T)
+XX = hat(LieAlgebra(G), Xⁱ)
+0.5*tr(XX*W*XX')
+inner(M, p, X, X)
+inner(base_manifold(G), p, X, X)
+
+
+end

src/variables/VariableTypes.jl

@@ -32,7 +32,7 @@ $(TYPEDEF)
 
 Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM.
 """
-@defVariable Pose2 SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0])
+@defVariable Pose2 SOnxRn_MetricManifold(2) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0])
 
 """
 $(TYPEDEF)