Use LieGroups.MetricLieGroup and other pose related updates

.github/workflows/ci.yml

@@ -4,73 +4,69 @@ on:
   push:
     branches:
       - develop
-      - release**
+      - 'release/**'
   workflow_dispatch:
+
 jobs:
   test:
-    name: Julia ${{ matrix.julia-version }} - ${{ matrix.os }} - ${{ matrix.arch }}
+    name: Julia ${{ matrix.julia-version }} - Env:${{ matrix.environment }} - ${{ matrix.os }}
     runs-on: ${{ matrix.os }}
     env:
       JULIA_PKG_SERVER: ""
+      JULIA_PROJECT: ${{ matrix.project-path }}
     strategy:
       fail-fast: false
       matrix:
-        julia-version: ['1.11', 'nightly']
+        julia-version: ['1', 'nightly']
         os: [ubuntu-latest]
         arch: [x64]
+        environment: ['standard', 'upstream']
+
+        include:
+          - environment: 'standard'
+            project-path: 'RoME/test'
+          - environment: 'upstream'
+            project-path: 'RoME/test/upstream_test'
+
+        exclude:
+          - environment: 'upstream'
+            julia-version: 'nightly'
+
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v4
+
       - uses: julia-actions/setup-julia@v2
         with:
           version: ${{ matrix.julia-version }}
           arch: ${{ matrix.arch }}
-      - uses: actions/cache@v4
-        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 }}-
-      - uses: julia-actions/julia-buildpkg@latest
+
+      - uses: julia-actions/cache@v2
+
+      # Julia automatically spins up in the folder specified by JULIA_PROJECT above.
+      - name: Instantiate workspace
+        shell: julia --color=yes {0}
+        run: |
+          using Pkg
+          Pkg.instantiate()
+
       - run: |
           git config --global user.name Tester
           git config --global user.email te@st.er
-      - uses: julia-actions/julia-runtest@latest
+
+      - name: Run Test Suite
+        shell: julia --color=yes {0}
+        run: |
+          using Pkg
+          Pkg.test("RoME"; coverage = true)
         continue-on-error: ${{ matrix.julia-version == 'nightly' }}
+
       - uses: julia-actions/julia-processcoverage@v1
-        if: ${{ matrix.julia-version == '1.10' }}
-      - uses: codecov/codecov-action@v1
         with:
-          file: lcov.info
-
-  upstream-dev:
-    #if: github.ref != 'refs/heads/release**'
-    name: Upstream Dev
-    runs-on: ubuntu-latest
-    env:
-      JULIA_PKG_SERVER: ""
-    steps:
-      - uses: actions/checkout@v4
-      - uses: julia-actions/setup-julia@v2
+          directories: RoME/src,RoME/ext,RoMETypes/src
+        if: ${{ matrix.julia-version == '1' }}
+
+      - uses: codecov/codecov-action@v4
+        if: ${{ matrix.julia-version == '1' }}
         with:
-          version: '1'
-          arch: x64
-      - uses: julia-actions/cache@v2
-      - name: Dev dependencies
-        shell: julia --color=yes --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="RoMETypes"), PackageSpec(path="RoME")])
-      - name: Run tests
-        env:
-          IIF_TEST_GROUP: 'basic_functional_group'
-        run: >
-          julia --color=yes --project=monorepo -e 'using Pkg; Pkg.test("RoME")'
-
+          file: lcov.info
+          token: ${{ secrets.CODECOV_TOKEN }}

Project.toml

@@ -0,0 +1,6 @@
+[workspace]
+projects = [
+    "RoMETypes",
+    "RoME",
+    "RoME/test"
+]

RoME/LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2026
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

RoME/Project.toml

@@ -58,14 +58,14 @@ Dates = "1.10"
 DelimitedFiles = "1"
 DifferentialEquations = "7"
 Distributed = "1.10"
-DistributedFactorGraphs = "0.28"
+DistributedFactorGraphs = "0.29"
 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.36.0"
+IncrementalInference = "0.37.0"
 IncrementalInferenceTypes = "0.1.0"
 Interpolations = "0.14, 0.15"
 KernelDensityEstimate = "0.5.1, 0.6"
@@ -78,7 +78,7 @@ OrderedCollections = "1"
 PrecompileTools = "1"
 ProgressMeter = "1"
 Random = "1.10"
-RecursiveArrayTools = "3"
+RecursiveArrayTools = "3, 4"
 Reexport = "1"
 RoMETypes = "0.1.0"
 Rotations = "1.1"
@@ -89,12 +89,5 @@ TimeZones = "1"
 TransformUtils = "0.2.17"
 julia = "1.10"
 
-[extras]
-CameraModels = "0d57b887-6120-40e6-8b8c-29d3116bc0c1"
-DifferentialEquations = "0c46a032-eb83-5123-abaf-570d42b7fbaa"
-Flux = "587475ba-b771-5e3f-ad9e-33799f191a9c"
-ImageIO = "82e4d734-157c-48bb-816b-45c225c6df19"
-Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
-
-[targets]
-test = ["CameraModels", "DifferentialEquations", "Flux", "ImageIO", "Test"]
+[workspace]
+projects = ["test"]

RoME/ext/RoMECameraModelsExt.jl

@@ -105,7 +105,7 @@ function solveMultiviewLandmark!(
     _c_Xi = Vector{CameraModels.PixelIndex{true, Float64}}()
     for fl in flbs
         vl = setdiff(ls(dfg, fl), [lmlb;])[1]
-        push!(_w_P_ci, getState(getVariable(dfg, vl), :parametric).val[1])
+        push!(_w_P_ci, DFG.refMeans(getState(getVariable(dfg, vl), :parametric))[1])
         union!(vlbs, [vl;])
 
         # TODO apply camera intrinsic calibration
@@ -121,7 +121,7 @@ function solveMultiviewLandmark!(
         w_Ph0 = [
             (
                 retry * randn(3) .+
-                getState(getVariable(dfg, lmlb), :parametric).val[1]
+                DFG.refMeans(getState(getVariable(dfg, lmlb), :parametric))[1]
             )...
             1.0
         ]

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.1]) .^ 2)),
+                )
+                sampleNoise ? MvNormal(rand(d), d.Σ) : d
+            end,
         )
         addFactor!(fg, [psym; nsym], pp; graphinit = graphinit)
     end

RoME/src/factors/PartialPose3.jl

@@ -119,12 +119,12 @@ end
 # Pose3Pose3XYYaw(z::SamplableBelief) = Pose3Pose3XYYaw(z, (1,2,4,5,6))   # (1,2,6))
 Pose3Pose3XYYaw(z::SamplableBelief) = Pose3Pose3XYYaw(z, (1, 2, 6))
 
-DFG.getManifold(::Type{<:Pose3Pose3XYYaw}) = SOnxRn_MetricManifold(2)
+DFG.getManifold(::Type{<:Pose3Pose3XYYaw}) = LeftInvariantMetricSE(2)
 
 ## NOTE, Yaw only works if you assume a preordained global reference point, such as identity_element(Pose3)
 function (cfo::CalcFactor{<:Pose3Pose3XYYaw})(X, wTp, wTq)
     #
-    M = SOnxRn_MetricManifold(2)
+    M = LeftInvariantMetricSE(2)
 
     rx = normalize(view(wTp.x[2], 1:2, 1))
     R = SA[

RoME/src/factors/Pose2D.jl

@@ -29,7 +29,7 @@ Base.@kwdef struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldM
     Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0]))
 end
 
-DFG.getManifold(::InstanceType{Pose2Pose2}) = SOnxRn_MetricManifold(2)
+DFG.getManifold(::InstanceType{Pose2Pose2}) = LeftInvariantMetricSE(2)
 
 Pose2Pose2(::UniformScaling) = Pose2Pose2()
 
@@ -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/Pose2Point2.jl

@@ -22,7 +22,7 @@ DFG.getManifold(::InstanceType{Pose2Point2}) = getManifold(Point2)
 # define the conditional probability constraint
 function (cfo::CalcFactor{<:Pose2Point2})(p_Xpq, w_T_p, w_Tl_q)
     #
-    M = SOnxRn_MetricManifold(2)
+    M = LeftInvariantMetricSE(2)
 
     p_T_qhat = ArrayPartition(SA[p_Xpq[1]; p_Xpq[2]], SMatrix{2, 2}([1 0; 0 1.0]))
     _w_T_p = ArrayPartition(SA[w_T_p.x[1]...], SMatrix{2, 2}(w_T_p.x[2]))