Update to LieGroups.jl

.github/workflows/ci.yml

@@ -46,39 +46,31 @@ jobs:
         with:
           file: lcov.info
 
-  test-masters:
+  upstream-dev:
     #if: github.ref != 'refs/heads/release**'
     name: Upstream Dev
     runs-on: ubuntu-latest
     env:
       JULIA_PKG_SERVER: ""
     steps:
-      - uses: actions/checkout@v2
-
+      - uses: actions/checkout@v4
       - uses: julia-actions/setup-julia@v2
         with:
-          version: '1.10'
+          version: '1'
           arch: x64
-
-      - uses: actions/cache@v4
+      - uses: julia-actions/cache@v2
+      - name: Dev dependencies
+        shell: julia --project=monorepo {0}
+        run: |
+          using Pkg;
+          Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"))
+          Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"))
+          Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInferenceTypes"))
+          Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInference"))
+          Pkg.develop(PackageSpec(path="."))
+      - name: Run tests
         env:
-          cache-name: cache-artifacts
-        with:
-          path: ~/.julia/artifacts
-          key: ${{ runner.os }}-test-${{ env.cache-name }}-${{ hashFiles('**/Project.toml') }}
-          restore-keys: |
-            ${{ runner.os }}-test-${{ env.cache-name }}-
-            ${{ runner.os }}-test-
-            ${{ runner.os }}-
+          IIF_TEST_GROUP: 'basic_functional_group'
+        run: >
+          julia --color=yes --project=monorepo -e 'using Pkg; Pkg.test("RoME")'
 
-      - run: |
-          git config --global user.name Tester
-          git config --global user.email te@st.er
-
-      - name: Run tests on Upstream Dev
-        run: |
-          julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="IncrementalInference",rev="develop"));'
-          julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));'
-          julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"));'
-          julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.test("RoME"; coverage=false)'
-        shell: bash

.gitignore

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

Project.toml

@@ -16,8 +16,10 @@ DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
+IncrementalInferenceTypes = "9808408f-4dbc-47e4-913c-6068b950e289"
 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 +57,18 @@ 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.0"
+IncrementalInferenceTypes = "0.1.0"
 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"
@@ -91,4 +95,4 @@ JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["CameraModels", "DifferentialEquations", "Flux", "ImageIO", "JSON3", "Test"]
+test = ["CameraModels", "DifferentialEquations", "Flux", "JSON3", "Test"]

RoMETypes/Project.toml

@@ -0,0 +1,24 @@
+name = "RoMETypes"
+uuid = "298dcef3-e3bd-403a-988f-2dd5ca311a3f"
+authors = ["NavAbility(TM) Contributors <info@wherewhen.ai>"]
+version = "0.1.0"
+
+[deps]
+DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04"
+DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f"
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
+ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
+RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
+StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
+
+[compat]
+DistributedFactorGraphs = "0.27.0"
+DocStringExtensions = "0.9.3"
+LieGroups = "0.1.3"
+LinearAlgebra = "1.11.0"
+Manifolds = "0.10"
+ManifoldsBase = "1.2.0"
+RecursiveArrayTools = "3.27.0"
+StaticArrays = "1.9.7"

RoMETypes/src/RoMETypes.jl

@@ -0,0 +1,52 @@
+module RoMETypes
+
+using DistributedFactorGraphs
+using DocStringExtensions
+using LieGroups
+using RecursiveArrayTools
+using StaticArrays
+using LinearAlgebra
+
+using ManifoldsBase: submanifold_components, TangentSpaceType, AbstractBasis, RiemannianMetric
+using Manifolds: MetricManifold
+
+import ManifoldsBase
+import Manifolds 
+
+export 
+    Point2,
+    Point3,
+    Pose2,
+    Pose3,
+    Rotation3,
+    RotVelPos,
+    VelPos3,
+    DynPoint2,
+    DynPose2,
+    projectCartesian
+
+export 
+    PriorPoint2,
+    PackedPriorPoint2,
+    PriorPoint3,
+    PackedPriorPoint3,
+    Pose2Pose2,
+    PackedPose2Pose2,
+    Pose3Pose3,
+    PackedPose3Pose3,
+    PriorPose3,
+    PackedPriorPose3,    
+    Point3Point3,
+    PackedPoint3Point3,
+    Point2Point2,
+    PackedPoint2Point2,
+    PriorPose2,
+    PackedPriorPose2
+
+export SOnxRn_MetricManifold
+
+include("manifolds/SOnxRn_MetricManifold.jl")
+include("variables/VariableTypes.jl")
+include("factors/FactorTypes.jl")
+
+end # module RoMETypes

RoMETypes/src/factors/FactorTypes.jl

@@ -0,0 +1,64 @@
+# ========================================================================================
+# Pose
+# ========================================================================================
+
+"""
+$(TYPEDEF)
+
+Direction observation information of a `Point3` variable.
+"""
+DFG.@defObservationType PriorPoint3 PriorObservation LieGroups.TranslationGroup(3)
+DFG.@defObservationType Point3Point3 RelativeObservation LieGroups.TranslationGroup(3)
+
+
+DFG.@defObservationType Point2Point2 RelativeObservation LieGroups.TranslationGroup(2)
+DFG.@defObservationType PriorPoint2 PriorObservation LieGroups.TranslationGroup(2)
+
+# ========================================================================================
+# Pose
+# ========================================================================================
+
+"""
+$(TYPEDEF)
+
+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)
+\\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``\\
+``X^i`` coordinates of ``X``
+
+DevNotes
+- Maybe with Manifolds.jl, `{T <: IIF.SamplableBelief, S, R, P}`
+
+Related
+
+[`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref)
+"""
+DFG.@defObservationType Pose2Pose2 RelativeObservation SOnxRn_MetricManifold(2)
+
+"""
+$(TYPEDEF)
+
+Introduce direct observations on all dimensions of a Pose2 variable:
+
+Example:
+--------
+```julia
+PriorPose2( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) )
+```
+"""
+DFG.@defObservationType PriorPose2 PriorObservation TranslationGroup(2) × SpecialOrthogonalGroup(2)
+
+DFG.@defObservationType Pose3Pose3 RelativeObservation SOnxRn_MetricManifold(3)
+DFG.@defObservationType PriorPose3 PriorObservation TranslationGroup(3) × SpecialOrthogonalGroup(3)

RoMETypes/src/manifolds/SOnxRn_MetricManifold.jl

@@ -0,0 +1,87 @@
+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
+ManifoldsBase.submanifold_component(G::MetricLieGroup, args...) = submanifold_component(G.lie_group, args...)
+ManifoldsBase.submanifold_components(G::MetricLieGroup, args...) = submanifold_components(G.lie_group, args...)
+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...)
+LieGroups.identity_element(G::MetricLieGroup, args...) = identity_element(base_manifold(G), args...)
+LieGroups.identity_element!(G::MetricLieGroup, args...) = identity_element!(base_manifold(G), args...)
+
+
+# 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
+
+function SOnxRn_MetricManifold(n)
+    MetricLieGroup(
+        SpecialEuclideanGroup(n; variant = :right),
+        LeftInvariantKinematicMetric(),
+    )
+end
+
+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, X)
+    G = base_manifold(M)
+    ε = identity_element(M, typeof(X))
+    return exp(base_manifold(G), ε, X)
+end
+
+function Manifolds.exp!(M::SOnxRn_MetricManifoldType, g, X)
+    G = base_manifold(M)
+    ε = identity_element(M, typeof(g))
+    return exp!(base_manifold(G), g, ε, X)
+end
+
+function ManifoldsBase.log(M::SOnxRn_MetricManifoldType, p)
+    G = base_manifold(M)
+    # ε = identity_element(G, typeof(p))
+    # X = log(base_manifold(G), ε, p)
+    PG = ProductLieGroup(map(LieGroup, G.manifold.manifolds, G.op.operations)...)
+    X = log(PG, p)
+    return X
+end
+function Manifolds.log!(M::SOnxRn_MetricManifoldType, X, p)
+    G = base_manifold(M)
+    ε = identity_element(M, typeof(p))
+    log!(base_manifold(G), X, ε, p)
+    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 DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(2)))
+    ArrayPartition(SA[0; 0.0], SA[1 0; 0 1.0])
+end
+function DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(3)))
+    ArrayPartition(SA[0, 0, 0.0], SA[1 0 0; 0 1 0; 0 0 1.0])
+end
+
+# LieGroups.LieAlgebra(G::SOnxRn_MetricManifoldType) = LieAlgebra(base_manifold(G))