Use LieGroups.MetricLieGroup and other pose related updates

Author: Affie

RoME/examples/Beehive2D.jl

@@ -504,7 +504,7 @@ plotTreeProductDown(csmcStep19.cliqSubFg, csmcStep19.tree, :x4, :x5)
 printCliqHistorySummary(tree, :x3)
 csmc = getCliqSolveHistory(tree, :x3)[9][4]
 
-getSolverParams(csmc.dfg).dbg = true
+IIF.getCliqueSolverParams(csmc).dbg = true
 
 stuff = sandboxCliqResolveStep(tree, :x3, 9)
 

RoME/src/canonical/GenerateCircular.jl

@@ -38,6 +38,7 @@ function generateGraph_Circle(
     biasTurn::Real = 0.0,
     kappaOdo::Real = 1.0,
     cyclePoses::Int = poses,
+    sampleNoise::Bool = false,
 )
     # assume empty factor graph object fg
     @assert offsetPoses < poses "`offsetPoses` must be smaller than total number of `poses`"
@@ -69,10 +70,12 @@ function generateGraph_Circle(
         addVariable!(fg, nsym, Pose2)
         # TODO rework the addition of noise alongside simulated ground truth, maybe `noise_t_cb`.
         pp = Pose2Pose2(
-            MvNormal(
-                [10.0; 0; 2pi / (cyclePoses) + biasTurn],
-                Matrix(Diagonal((kappaOdo * [0.1; 0.1; 0.1]) .^ 2)),
-            ),
+            let d = MvNormal(
+                    [10.0; 0; 2pi / (cyclePoses) + biasTurn],
+                    Matrix(Diagonal((kappaOdo * [0.1; 0.1; 0.01]) .^ 2)),
+                )
+                sampleNoise ? MvNormal(rand(d), d.Σ) : d
+            end,
         )
         addFactor!(fg, [psym; nsym], pp; graphinit = graphinit)
     end

RoME/src/factors/Pose2D.jl

@@ -40,6 +40,17 @@ function (cf::CalcFactor{<:Pose2Pose2})(X, p, q)
     return vee(M, p, X - X̂) # TODO check sign
 end
 
+# An alternative error function that parallel transports the error vector back to p
+# function (cf::CalcFactor{<:RobustPose2Pose2})(X, p, q)
+#     G = getManifold(RobustPose2Pose2)
+#     q̂ = exp(G, p, X)
+#     E_q̂ = log(G, q̂, q)
+#     # Parallel transport the error vector back to p.
+#     E_p = parallel_transport_to(G, q̂, E_q̂, p)    
+#     return vee(LieAlgebra(G), E_p) 
+# end
+
+
 # NOTE, serialization support -- will be reduced to macro in future
 # ------------------------------------
 

RoME/src/factors/Poses.jl

@@ -28,11 +28,11 @@ function (cf::CalcFactor{<:PriorPose3})(m, p)
     return vee(LieAlgebra(M), log(M, p, m))
 end
 
-function (cf::CalcFactor{<:Pose3Pose3})(X, p::ArrayPartition{T}, q) where {T}
+function (cf::CalcFactor{<:Pose3Pose3})(X, p, q)
+    # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M
     M = getManifold(Pose3Pose3)
     X̂ = log(M, p, q)
-    Xc::SVector{6, T} = vee(LieAlgebra(M), X - X̂)
-    return Xc
+    return vee(LieAlgebra(M), X - X̂)
 end
 
 # FIXME, rather have separate compareDensity functions

RoMETypes/Project.toml

@@ -16,7 +16,7 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 [compat]
 DistributedFactorGraphs = "0.27, 0.28.0"
 DocStringExtensions = "0.9.3"
-LieGroups = "0.1.3"
+LieGroups = "0.1.11"
 LinearAlgebra = "1.11.0"
 Manifolds = "0.10, 0.11"
 ManifoldsBase = "1.2.0, 2"

RoMETypes/src/manifolds/SOnxRn_MetricManifold.jl

@@ -1,32 +1,4 @@
-struct MetricLieGroup{
-    𝔽,
-    O <: AbstractGroupOperation,
-    M <: ManifoldsBase.AbstractManifold{𝔽},
-    L <: LieGroup{𝔽, O, M},
-    G <: RiemannianMetric,
-} <: AbstractLieGroup{𝔽, O, M}
-    lie_group::L
-    metric::G
-end
-
-ManifoldsBase.base_manifold(G::MetricLieGroup) = G.lie_group
-function ManifoldsBase.submanifold_component(G::MetricLieGroup, args...)
-    return submanifold_component(G.lie_group, args...)
-end
-function ManifoldsBase.submanifold_components(G::MetricLieGroup, args...)
-    return submanifold_components(G.lie_group, args...)
-end
-LieGroups.LieAlgebra(G::MetricLieGroup) = LieAlgebra(base_manifold(G))
-LieGroups.inv!(G::MetricLieGroup, args...) = inv!(base_manifold(G), args...)
-LieGroups.inv(G::MetricLieGroup, args...) = inv(base_manifold(G), args...)
-LieGroups.compose!(G::MetricLieGroup, args...) = compose!(base_manifold(G), args...)
-LieGroups.compose(G::MetricLieGroup, args...) = compose(base_manifold(G), args...)
-function LieGroups.identity_element(G::MetricLieGroup, args...)
-    return identity_element(base_manifold(G), args...)
-end
-function LieGroups.identity_element!(G::MetricLieGroup, args...)
-    return identity_element!(base_manifold(G), args...)
-end
+using LieGroups: MetricLieGroup
 
 # Left Invariant Rigid Body Kinematics Metric CrokeKumar eq 61.
 # A family of left invariant metrics:
@@ -46,10 +18,13 @@ end
 SOnxRn_MetricManifoldType =
     Union{typeof(SOnxRn_MetricManifold(2)), typeof(SOnxRn_MetricManifold(3))}
 
+#
+LieGroups.LieAlgebra(G::SOnxRn_MetricManifoldType) = LieAlgebra(G.lie_group)
+
 # geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3
 function Manifolds.exp(M::SOnxRn_MetricManifoldType, X)
-    G = base_manifold(M)
-    ε = identity_element(M, typeof(X))
+    G = base_lie_group(M)
+    ε = identity_element(G, typeof(X))
     return exp(base_manifold(G), ε, X)
 end
 
@@ -60,7 +35,7 @@ function Manifolds.exp!(M::SOnxRn_MetricManifoldType, g, X)
 end
 
 function ManifoldsBase.log(M::SOnxRn_MetricManifoldType, p)
-    G = base_manifold(M)
+    G = base_lie_group(M)
     # ε = identity_element(G, typeof(p))
     # X = log(base_manifold(G), ε, p)
     PG = ProductLieGroup(map(LieGroup, G.manifold.manifolds, G.op.operations)...)