From 26cc1fe250f614626ea6fa546aad50fbef24e8e2 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <6612981+Affie@users.noreply.github.com>
Date: Mon, 1 Jun 2026 17:30:39 +0200
Subject: [PATCH 1/6] Backports for parametric numerics (#1930)

* Add DebugTension feature and pinv_subsolver to RLM functions

* Use tree for getInitOrderParametric and therefore autoinitParametric!

* fix julia compat

* update Project.toml

---------

Co-authored-by: Johannes Terblanche <Affie@users.noreply.github.com>
---
 .github/workflows/ci.yml                      |   2 +-
 IncrementalInference/Project.toml             |   7 +-
 .../services/CliqueStateMachine.jl            |   2 +-
 .../parametric/services/ParametricManopt.jl   | 206 ++++++++++++------
 .../parametric/services/ParametricUtils.jl    |  44 +++-
 .../src/services/JunctionTreeUtils.jl         |   6 +-
 .../test/testBasicParametric.jl               | 118 +++++++++-
 .../test/testSpecialEuclidean2Mani.jl         |   4 +-
 README.md                                     |   2 +-
 9 files changed, 307 insertions(+), 84 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 707d9dbd..ffca4800 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -17,7 +17,7 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - 'lts'
+          - '1'
         os:
           - ubuntu-latest
         arch:
diff --git a/IncrementalInference/Project.toml b/IncrementalInference/Project.toml
index d21467b4..7b4048a4 100644
--- a/IncrementalInference/Project.toml
+++ b/IncrementalInference/Project.toml
@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.37.0"
+version = "0.37.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -102,7 +102,7 @@ Optim = "1"
 OrderedCollections = "1"
 PrecompileTools = "1"
 ProgressMeter = "1"
-RecursiveArrayTools = "3"
+RecursiveArrayTools = "3, 4"
 Reexport = "1"
 SparseDiffTools = "2"
 StaticArrays = "1"
@@ -112,7 +112,8 @@ StructTypes = "1"
 TensorCast = "0.3.3, 0.4"
 TimeZones = "1.3.1"
 TimesDates = "0.3.3"
-julia = "1.10"
+UUIDs = "1.11.0"
+julia = "1.11"
 
 [extras]
 AMD = "14f7f29c-3bd6-536c-9a0b-7339e30b5a3e"
diff --git a/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl b/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl
index d15f4d01..d070486c 100644
--- a/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl
+++ b/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl
@@ -931,7 +931,7 @@ function updateFromSubgraph_StateMachine(csmc::CliqStateMachineContainer)
   if !isParametricSolve
     for sym in getCliqFrontalVarIds(csmc.cliq)
       # set solved flag
-      vari = getVariable(csmc.cliqSubFg, sym, csmc.solveKey)
+      vari = getVariable(csmc.cliqSubFg, sym)
       setSolvedCount!(vari, getSolvedCount(vari, csmc.solveKey) + 1, csmc.solveKey)
     end
   end
diff --git a/IncrementalInference/src/parametric/services/ParametricManopt.jl b/IncrementalInference/src/parametric/services/ParametricManopt.jl
index c1ee7073..f77268f6 100644
--- a/IncrementalInference/src/parametric/services/ParametricManopt.jl
+++ b/IncrementalInference/src/parametric/services/ParametricManopt.jl
@@ -1,4 +1,5 @@
 using Manopt
+using Manopt.Printf
 using FiniteDiff
 using SparseDiffTools
 using SparseArrays
@@ -389,6 +390,44 @@ function qr_linear_subsolver!(sk, JJ, grad_f_c)
   return sk
 end
 
+function pinv_subsolver!(sk, JJ, grad_f_c)
+  sk .= pinv(JJ) * grad_f_c
+  return sk
+end
+
+"""
+    DebugTension(dof; io=stdout)
+
+Manopt `DebugAction` that prints the tension (reduced chi-squared) at each iteration.
+Tension = 2*cost / dof, where `dof = N - M` (residual dimension minus state dimension).
+
+Usage in `solve_RLM`:
+```julia
+dof = num_components - manifold_dimension(M)
+solve_RLM(fg; debug = [:Iteration, " | ", DebugTension(dof), "\n", 1])
+```
+"""
+mutable struct DebugTension <: Manopt.DebugAction
+  dof::Int
+  io::IO
+  format::String
+end
+DebugTension(dof::Int; io::IO=stdout, format="tension: %.2f") = DebugTension(dof, io, format)
+
+function (d::DebugTension)(p::Manopt.AbstractManoptProblem, st::Manopt.AbstractManoptSolverState, k::Int)
+  if d.dof <= 0
+    s = NaN
+  else
+    cost = Manopt.get_cost(p, Manopt.get_iterate(st))
+    s = sqrt(2 * cost / d.dof)
+  end
+  Printf.format(d.io, Printf.Format(d.format), s)
+  return nothing
+end
+
+# replace :tension symbol with DebugTension(dof) in a debug vector
+_inject_tension(debug, dof) = map(x -> x === :tension ? DebugTension(dof) : x, debug)
+
 function solve_RLM(
   fg,
   varlabels = ls(fg),
@@ -440,6 +479,12 @@ function solve_RLM(
     zeros(num_components, manifold_dimension(M))
   end
 
+  # inject DebugTension for :tension symbol in debug kwarg
+  dof = num_components - manifold_dimension(M)
+  if haskey(kwargs, :debug)
+    kwargs = (; kwargs..., debug = _inject_tension(kwargs[:debug], dof))
+  end
+
   lm_r = Manopt.LevenbergMarquardt!(
     M,
     costF!,
@@ -463,7 +508,13 @@ function solve_RLM(
     Λ = Symmetric(J'J) # approx Hessian = precision matrix
   end
 
-  return M, varlabelsAP, lm_r, Λ
+  # tension (reduced chi-squared): ||r||^2 / (N-M) = 2*cost / (N-M)
+  dof = num_components - manifold_dimension(M)
+  final_res = zeros(num_components)
+  costF!(M, final_res, lm_r)
+  tension = dof > 0 ? sum(abs2, final_res) / dof : NaN
+
+  return M, varlabelsAP, lm_r, Λ, tension
 end
 
   # nlso = NonlinearLeastSquaresObjective(
@@ -524,6 +575,7 @@ function solve_RLM_conditional(
   finiteDiffCovariance=true,
   jacobian_method::Symbol = :finitediff,
   solveKey::Symbol = :parametric,
+  linear_subsolver! = qr_linear_subsolver!,
   kwargs...
 )
   is_sparse && error("Sparse solve_RLM_conditional not supported yet")
@@ -551,7 +603,7 @@ function solve_RLM_conditional(
     separator_varlabelsAP = ArrayPartition{Symbol,Tuple}(())
   else
     _, _, separator_vartypeslist = getVariableTypesCount(getVariable.(fg,separators))
-    seperator_varIntLabel, separator_varlabelsAP = getVarIntLabelMap(separator_vartypeslist)
+    separator_varIntLabel, separator_varlabelsAP = getVarIntLabelMap(separator_vartypeslist)
   end
 
   all_varlabelsAP = ArrayPartition((frontal_varlabelsAP.x..., separator_varlabelsAP.x...))
@@ -599,6 +651,12 @@ function solve_RLM_conditional(
     zeros(num_components, manifold_dimension(M))
   end
 
+  # inject DebugTension for :tension symbol in debug kwarg
+  dof = num_components - manifold_dimension(M)
+  if haskey(kwargs, :debug)
+    kwargs = (; kwargs..., debug = _inject_tension(kwargs[:debug], dof))
+  end
+
   lm_r = LevenbergMarquardt(
     M,
     costF!,
@@ -608,6 +666,7 @@ function solve_RLM_conditional(
     evaluation=InplaceEvaluation(),
     initial_residual_values,
     initial_jacobian_f,
+    linear_subsolver!,
     kwargs...
   )
 
@@ -617,8 +676,13 @@ function solve_RLM_conditional(
     jacF!(M, initial_jacobian_f, lm_r)
     Λ = Symmetric(initial_jacobian_f' * initial_jacobian_f)
   end
+
+  # tension (reduced chi-squared): ||r||^2 / (N-M) = 2*cost / (N-M)
+  final_res = zeros(num_components)
+  costF!(M, final_res, lm_r)
+  tension = sum(abs2, final_res) / dof
   
-  return M, frontal_varlabelsAP, lm_r, Λ
+  return M, frontal_varlabelsAP, lm_r, Λ, tension
 end
 
 function extractMarginalsAP(M, labelsAP::ArrayPartition{Symbol}, Σ::AbstractArray{<:Real})
@@ -642,12 +706,12 @@ end
   
 function autoinitParametric!(
   fg,
-  varorderIds = getInitOrderParametric(fg);
+  clique_order = getInitOrderParametric(fg);
   reinit = false,
   kwargs...
 )
-  init_labels = @showprogress map(varorderIds) do vIdx
-    autoinitParametric!(fg, vIdx; reinit, kwargs...)
+  init_labels = @showprogress map(clique_order) do cliq
+    autoinitParametric!(fg, cliq.frontals, cliq.separators; reinit, kwargs...)
   end
   filter!(!isnothing, init_labels)
   return init_labels
@@ -657,70 +721,93 @@ function autoinitParametric!(dfg::AbstractDFG, initme::Symbol; kwargs...)
   return autoinitParametric!(dfg, getVariable(dfg, initme); kwargs...)
 end
 
+function autoinitParametric!(dfg::AbstractDFG, xi::VariableCompute; solveKey = :parametric, kwargs...)
+  initme = getLabel(xi)
+  separators = ls2(dfg, initme)
+  filter!(separators) do vl
+    return hasState(dfg, vl, solveKey) && isInitialized(dfg, vl, solveKey)
+  end
+  return autoinitParametric!(dfg, [initme], separators; solveKey, kwargs...)
+end
+
 function autoinitParametric!(
   dfg::AbstractDFG,
-  xi::VariableCompute;
+  frontals::Vector{Symbol},
+  separators::Vector{Symbol} = Symbol[];
   solveKey = :parametric,
   reinit::Bool = false,
-  perturb_point::Bool=false,
+  linear_subsolver! = pinv_subsolver!,
   kwargs...,
 )
-  #
-
-  initme = getLabel(xi)
-  vnd = getState(xi, solveKey)
-  # don't initialize a variable more than once
-  if reinit || !isInitialized(xi, solveKey)
+  # Filter to only uninitialized variables (unless reinit)
+  to_init = if reinit
+    frontals
+  else
+    filter(v -> !isInitialized(dfg, v, solveKey), frontals)
+  end
+  isempty(to_init) && return false
 
-    # frontals - initme
-    # separators - inifrom
+  # Filter separators to only those already initialized
+  active_separators = filter(separators) do vl
+    hasState(dfg, vl, solveKey) && isInitialized(dfg, vl, solveKey)
+  end
 
-    initfrom = ls2(dfg, initme)
-    filter!(initfrom) do vl
-      return isInitialized(dfg, vl, solveKey)
-    end
-    
-    # nothing to initialize if no initialized neighbors or priors
-    if isempty(initfrom) && !any(isPrior.(dfg, listNeighbors(dfg, initme)))
-      return false
+  # Nothing to initialize if no separators and no priors on any frontal
+  if isempty(active_separators)
+    has_any_prior = any(to_init) do v
+      any(isPrior.(dfg, listNeighbors(dfg, v)))
     end
+    has_any_prior || return false
+  end
 
-    if perturb_point
-      _M = getManifold(xi)
-      p = DFG.refMeans(vnd)[1]
-      DFG.refMeans(vnd)[1] = exp(
-        _M,
-        p, 
-        get_vector(
-          _M,
-          p,
-          randn(manifold_dimension(_M))*10^-6,
-          LieGroups.DefaultLieAlgebraOrthogonalBasis()
-        )
-      )
+  # Check that we have usable factors
+  varlabels = union(to_init, active_separators)
+  _, faclabels = listNeighborhood(dfg, varlabels, 1)
+  filter!(fl -> issubset(getVariableOrder(dfg, fl), varlabels), faclabels)
+  isempty(faclabels) && return false
+
+  # Seed each frontal from an initialized separator of the same type
+  for v in to_init
+    xi = getVariable(dfg, v)
+    vnd = getState(xi, solveKey)
+    has_prior = any(isPrior.(dfg, listNeighbors(dfg, v)))
+    if !has_prior && !isempty(active_separators)
+      my_kind = getStateKind(xi)
+      same_kind = filter(active_separators) do vl
+        getStateKind(getVariable(dfg, vl)) === my_kind
+      end
+      if !isempty(same_kind)
+        DFG.refMeans(vnd)[1] = DFG.refMeans(getState(dfg, same_kind[1], solveKey))[1]
+      end
     end
-    M, vartypeslist, lm_r, Λ = solve_RLM_conditional(dfg, [initme], initfrom; solveKey, kwargs...)
-    
-    val = lm_r[1]
-    DFG.refMeans(vnd)[1] = val
+  end
 
-    if !isnothing(Λ)
-      DFG.refCovariances(vnd)[1] .= inv(Matrix(Λ))
-    end
-  
-    # updateSolverDataParametric!(vnd, val, Σ)
+  # Solve
+  M, varlabelsAP, lm_r, Λ, _ = solve_RLM_conditional(dfg, to_init, active_separators; solveKey, linear_subsolver!, kwargs...)
 
+  # Update each frontal variable with result
+  for (i, v) in enumerate(varlabelsAP)
+    vnd = getState(dfg, v, solveKey)
+    DFG.refMeans(vnd)[1] = lm_r[i]
     vnd.initialized = true
-    #fill in ppe as mean
-    Xc::Vector{Float64} = collect(getCoordinates(getStateKind(xi), val))
-
-    result = true
+  end
 
-  else
-    result = false
+  # Update covariances from joint precision if positive definite
+  if !isnothing(Λ)
+    F = cholesky!(Λ; check = false)
+    if issuccess(F)
+      Σ = F \ I(size(Λ, 1))
+      offset = 0
+      for (i, v) in enumerate(varlabelsAP)
+        dim = manifold_dimension(getManifold(getVariable(dfg, v)))
+        r = (offset + 1):(offset + dim)
+        DFG.refCovariances(getState(dfg, v, solveKey))[1] .= Σ[r, r]
+        offset += dim
+      end
+    end
   end
 
-  return result#isInitialized(xi, solveKey)
+  return true
 end
 
 
@@ -734,7 +821,7 @@ solveGraphParametric(args...; kwargs...) = solve_RLM(args...; kwargs...)
 function DFG.solveGraphParametric!(
   fg::AbstractDFG,
   args...; 
-  init::Bool = false, 
+  init::Bool = true, 
   solveKey::Symbol = :parametric,
   is_sparse = true,
   # debug, stopping_criterion, damping_term_min=1e-2, 
@@ -743,17 +830,13 @@ function DFG.solveGraphParametric!(
 )
   # make sure variables has solverData, see #1637
   makeSolverData!(fg; solveKey)
-  if !(:parametric in fg.solverParams.algorithms)
-    addParametricSolver!(fg; init = init)
-  elseif init
-    error("TODO: not implemented")
-  end
+  init && autoinitParametric!(fg; solveKey)
 
-  M, v, r, Λ = solve_RLM(fg, args...; is_sparse, kwargs...)
+  M, v, r, Λ, tension = solve_RLM(fg, args...; is_sparse, kwargs...)
 
   updateParametricSolution!(fg, M, v, r, Λ)
 
-  return M, v, r, Λ 
+  return M, v, r, Λ
 end
 
 
@@ -803,4 +886,3 @@ function (cost::CostF_RLM_WRAP2!)(M::AbstractManifold, x::Vector{T}, p::Abstract
   return x
 end
 =#
-
diff --git a/IncrementalInference/src/parametric/services/ParametricUtils.jl b/IncrementalInference/src/parametric/services/ParametricUtils.jl
index 1eed6d1e..c3f8a1ae 100644
--- a/IncrementalInference/src/parametric/services/ParametricUtils.jl
+++ b/IncrementalInference/src/parametric/services/ParametricUtils.jl
@@ -983,24 +983,52 @@ function createMvNormal(v::VariableCompute, key = :parametric)
 end
 
 #TODO this is still experimental and a POC
-function getInitOrderParametric(fg; startIdx::Symbol = lsfPriors(fg)[1])
-  order = DFG.traverseGraphTopologicalSort(fg, startIdx)
-  filter!(order) do l
-    return isVariable(fg, l)
+"""
+    $SIGNATURES
+
+Build the Bayes tree for `fg` and return a vector of `(frontals, separators)` tuples
+ordered root-to-leaves (BFS).
+"""
+function getInitOrderParametric(fg; ordering::Symbol = :qr)
+  tree = buildTreeReset!(fg; ordering)
+
+  # BFS root-to-leaves: parents are processed before children
+  clique_order = Vector{NamedTuple{(:frontals, :separators), Tuple{Vector{Symbol}, Vector{Symbol}}}}()
+  
+  # Find root cliques
+  queue = TreeClique[]
+  for cliqId in getCliqueIds(tree)
+    if isRoot(tree, cliqId)
+      push!(queue, getClique(tree, cliqId))
+    end
+  end
+
+  while !isempty(queue)
+    cliq = popfirst!(queue)
+    frontals = getCliqFrontalVarIds(cliq)
+    separators = getCliqSeparatorVarIds(cliq)
+    push!(clique_order, (; frontals, separators))
+    # Enqueue children
+    for child in getChildren(tree, cliq)
+      push!(queue, child)
+    end
   end
-  return order
+
+  return clique_order
 end
 
 function autoinitParametricOptim!(
   fg,
-  varorderIds = getInitOrderParametric(fg);
+  clique_order = getInitOrderParametric(fg);
   reinit = false,
   algorithm = Optim.NelderMead,
   algorithmkwargs = (initial_simplex = Optim.AffineSimplexer(0.025, 0.1),),
   kwargs...
 )
-  @showprogress for vIdx in varorderIds
-    autoinitParametricOptim!(fg, vIdx; reinit, algorithm, algorithmkwargs, kwargs...)
+  @showprogress for cliq in clique_order
+    for vIdx in cliq.frontals
+      autoinitParametricOptim!(fg, vIdx; reinit, algorithm, algorithmkwargs, kwargs...)
+    end
   end
   return nothing
 end
diff --git a/IncrementalInference/src/services/JunctionTreeUtils.jl b/IncrementalInference/src/services/JunctionTreeUtils.jl
index d4263ed6..09c175be 100644
--- a/IncrementalInference/src/services/JunctionTreeUtils.jl
+++ b/IncrementalInference/src/services/JunctionTreeUtils.jl
@@ -778,7 +778,7 @@ function buildTreeFromOrdering!(
   # copy required for both remote and local graphs
   DFG.deepcopyGraph!(fge, dfg)
 
-  @info "Building Bayes net..."
+  @debug "Building Bayes net..."
   buildBayesNet!(fge, elimOrder; solvable = solvable)
 
   tree = BayesTree()
@@ -793,7 +793,7 @@ function buildTreeFromOrdering!(
     close(fid)
   end
 
-  @info "Find potential functions for each clique"
+  @debug "Find potential functions for each clique"
   for cliqIds in getCliqueIds(tree)
     # start at the root, of which there could be multiple disconnected trees
     if isRoot(tree, cliqIds)
@@ -849,7 +849,7 @@ function prepBatchTreeOLD!(
 
   tree = buildTreeFromOrdering!(dfg, Symbol.(p); drawbayesnet = false) # drawbayesnet
 
-  @info "Bayes Tree Complete"
+  @debug "Bayes Tree Complete"
   if drawpdf
     drawTree(tree; show = show, filepath = filepath, viewerapp = viewerapp, imgs = imgs)
   end
diff --git a/IncrementalInference/test/testBasicParametric.jl b/IncrementalInference/test/testBasicParametric.jl
index 7f2ba806..6443a164 100644
--- a/IncrementalInference/test/testBasicParametric.jl
+++ b/IncrementalInference/test/testBasicParametric.jl
@@ -2,8 +2,8 @@ using Test
 
 using DistributedFactorGraphs
 using IncrementalInference
-
-##
+using LieGroups
+using LinearAlgebra
 
 @testset "Test consolidation of factors #467" begin
   fg = generateGraph_LineStep(20, poseEvery=1, landmarkEvery=4, posePriorsAt=collect(0:7), sightDistance=2, solverParams=SolverParams(algorithms=[:default, :parametric]))
@@ -271,5 +271,117 @@ initAll!(fg, :parametric)
 ##
 end
 
+# test/testParametricUninitializable.jl
+## Define a ternary "biased relative" factor: x_j = x_i + z + b
+# The bias `b` is only observable through these factors (no direct prior).
+struct BiasedLinearRelative{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
+    Z::T
+end
 
-#
+DFG.getManifold(::IIF.InstanceType{BiasedLinearRelative}) = LieGroups.TranslationGroup(1)
+
+# residual: z - (x2 - x1 - b)
+function (cf::CalcFactor{<:BiasedLinearRelative})(z, x1, x2, b)
+    return z .- (x2 .- x1 .- b)
+end
+
+##
+@testset "Parametric: uninitializable variable (ternary bias factor)" begin
+    fg = initfg()
+    fg.solverParams.graphinit = false
+
+    # Chain: x0 --[biased]--> x1 --[biased]--> x2
+    # with shared bias variable :b (no prior on :b)
+    addVariable!(fg, :x0, ContinuousScalar)
+    addVariable!(fg, :x1, ContinuousScalar)
+    addVariable!(fg, :x2, ContinuousScalar)
+    addVariable!(fg, :b, ContinuousScalar)
+
+    addFactor!(fg, [:x0], Prior(Normal(0.0, 0.1)))
+    addFactor!(fg, [:x2], Prior(Normal(2.5, 0.1)))
+
+    # Biased relative factors: x1 = x0 + 1.0 + b, x2 = x1 + 1.0 + b
+    # True solution: 1.0+b → 2*(1.0+b)=2.5 → b=0.25
+    addFactor!(fg, [:x0, :x1, :b], BiasedLinearRelative(Normal(1.0, 0.1)))
+    addFactor!(fg, [:x1, :x2, :b], BiasedLinearRelative(Normal(1.0, 0.1)))
+
+    # autoinitParametric! should NOT crash on :b even though it's locally under-constrained
+    IIF.autoinitParametric!(fg)
+
+    # The global parametric solve should still work and find the correct solution
+    M, v, r, Λ = IIF.solveGraphParametric!(fg; init=false)
+
+    x0 = DFG.refMeans(getState(fg, :x0, :parametric))[1]
+    x1 = DFG.refMeans(getState(fg, :x1, :parametric))[1]
+    x2 = DFG.refMeans(getState(fg, :x2, :parametric))[1]
+    b  = DFG.refMeans(getState(fg, :b, :parametric))[1]
+
+    @test isapprox(x0[1], 0.0, atol=0.05)
+    @test isapprox(x2[1], 2.5, atol=0.05)
+    @test isapprox(b[1], 0.25, atol=0.05)
+    @test isapprox(x1[1], x0[1] + 1.0 + b[1], atol=0.05)
+end
+
+"""
+    PartialExpCoordPrior
+
+A partial prior that constrains specific exponential coordinates of a Lie group variable.
+
+Mathematically, this factor applies a prior to a subset of the tangent coordinates `vee(log(G, g))`. 
+It acts as a locally valid submersion on any Lie group, provided the variable remains within 
+the injectivity radius where the parameterization in exponential coordinates is well-defined.
+The `partial` tuple selects which coordinates of the vee representation are observed.
+"""
+struct PartialExpCoordPrior{G <: LieGroups.AbstractLieGroup, T <: IIF.SamplableBelief, P <: Tuple} <: IIF.AbstractPriorObservation
+    G::G
+    Z::T
+    partial::P
+end
+
+# Factor manifold is the residual space: ℝ^k where k = length(partial)
+DFG.getManifold(pp::PartialExpCoordPrior) = LieGroups.TranslationGroup(length(pp.partial))
+
+function (cf::CalcFactor{<:PartialExpCoordPrior})(z, x1)
+    G = cf.factor.G
+    # Get exponential coordinates
+    X = log(G, x1) 
+    Xc = vee(LieAlgebra(G), X)
+    return z .- Xc[collect(cf.factor.partial)]   # Residual on selected coords
+end
+
+@testset "Parametric: PartialExpCoordPrior on 2D variable (locally rank-deficient)" begin
+    fg = initfg()
+    fg.solverParams.graphinit = false
+
+    G = LieGroups.TranslationGroup(2)
+
+    # x0 has partial prior on x-coord only (y unconstrained locally)
+    # x2 has partial prior on y-coord only (x unconstrained locally)
+    # LinearRelative{2} chain makes the full graph solvable
+    addVariable!(fg, :x0, ContinuousEuclid{2})
+    addVariable!(fg, :x1, ContinuousEuclid{2})
+    addVariable!(fg, :x2, ContinuousEuclid{2})
+
+    # x0: only x-coordinate known via partial prior on coord 1
+    addFactor!(fg, [:x0], PartialExpCoordPrior(G, Normal(0.0, 0.1), (1,)))
+    # x2: only y-coordinate known via partial prior on coord 2
+    addFactor!(fg, [:x2], PartialExpCoordPrior(G, Normal(3.0, 0.1), (2,)))
+
+    # Relative factors that constrain both dimensions
+    addFactor!(fg, [:x0, :x1], LinearRelative{2}(MvNormal([1.0, 1.0], 0.1*I(2))))
+    addFactor!(fg, [:x1, :x2], LinearRelative{2}(MvNormal([1.0, 1.0], 0.1*I(2))))
+
+    IIF.autoinitParametric!(fg)
+
+    M, v, r, Λ = IIF.solveGraphParametric!(fg; init=false)
+
+    x0 = DFG.refMeans(getState(fg, :x0, :parametric))[1]
+    x1 = DFG.refMeans(getState(fg, :x1, :parametric))[1]
+    x2 = DFG.refMeans(getState(fg, :x2, :parametric))[1]
+
+    # x0[1] ≈ 0.0 (from prior), x2[2] ≈ 3.0 (from prior)
+    # Propagation: x0[2] = x2[2] - 2.0 = 1.0, x2[1] = x0[1] + 2.0 = 2.0
+    @test isapprox(x0, [0.0, 1.0], atol=0.05)
+    @test isapprox(x1, [1.0, 2.0], atol=0.05)
+    @test isapprox(x2, [2.0, 3.0], atol=0.05)
+end
diff --git a/IncrementalInference/test/testSpecialEuclidean2Mani.jl b/IncrementalInference/test/testSpecialEuclidean2Mani.jl
index 680c69ae..b6a8882e 100644
--- a/IncrementalInference/test/testSpecialEuclidean2Mani.jl
+++ b/IncrementalInference/test/testSpecialEuclidean2Mani.jl
@@ -122,7 +122,7 @@ smtasks = Task[]
 result = solveTree!(fg; smtasks, verbose=true)
 @test result isa AbstractBayesTree
 
-IIF.solveGraphParametric!(fg; sparse = false, damping_term_min=1e-12)
+IIF.solveGraphParametric!(fg; is_sparse = false, damping_term_min=1e-12)
 
 vnd = getState(fg, :x0, :parametric)
 @test all(isapprox(M, DFG.refMeans(vnd)[1], p0, atol=1e-6))
@@ -209,7 +209,7 @@ addFactor!(fg, [:x6; :l1], mf)
 smtasks = Task[]
 solveTree!(fg; smtasks);
 IIF.autoinitParametric!(fg)
-IIF.solveGraphParametric!(fg; sparse = false, damping_term_min=1e-12)
+IIF.solveGraphParametric!(fg; is_sparse = false, damping_term_min=1e-12)
 
 vnd = getState(fg, :x0, :default)
 @test isapprox(M, mean(M, DFG.refPoints(vnd)), ArrayPartition([10.0,10.0], [-1.0 0.0; 0.0 -1.0]), atol=0.2)
diff --git a/README.md b/README.md
index 6b6c8e2c..a5da6fc1 100644
--- a/README.md
+++ b/README.md
@@ -71,7 +71,7 @@ sudo apt-get install graphviz xdot # optional
 [iif-ci-dev-img]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml/badge.svg
 [iif-ci-dev-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml?query=branch%3Adevelop
 [iif-ci-stb-img]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml/badge.svg
-[iif-ci-stb-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml?query=branch%3Arelease%2Fv0.35
+[iif-ci-stb-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml?query=branch%3Arelease%2Fv0.37
 [iif-ver-img]: https://juliahub.com/docs/IncrementalInference/version.svg
 [iif-rel-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/releases
 [iif-milestones]: https://github.com/JuliaRobotics/IncrementalInference.jl/milestones

From 7668aa5dcb9ae66a565108febd75435353521a60 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <6612981+Affie@users.noreply.github.com>
Date: Sun, 7 Jun 2026 18:17:22 +0200
Subject: [PATCH 2/6] Improve autoinitParametric! (#1936)

* Improve autoinitParametric!
* Bump version to v0.37.2
* Add graph based initialization order option autoinitParametric!
---
 IncrementalInference/Project.toml             |  2 +-
 .../parametric/services/ParametricManopt.jl   | 54 +++++++++++++++++--
 2 files changed, 50 insertions(+), 6 deletions(-)

diff --git a/IncrementalInference/Project.toml b/IncrementalInference/Project.toml
index 7b4048a4..c2f4ed37 100644
--- a/IncrementalInference/Project.toml
+++ b/IncrementalInference/Project.toml
@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.37.1"
+version = "0.37.2"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
diff --git a/IncrementalInference/src/parametric/services/ParametricManopt.jl b/IncrementalInference/src/parametric/services/ParametricManopt.jl
index f77268f6..4d75b1a6 100644
--- a/IncrementalInference/src/parametric/services/ParametricManopt.jl
+++ b/IncrementalInference/src/parametric/services/ParametricManopt.jl
@@ -704,17 +704,50 @@ end
   # new2    0.010764 seconds (34.61 k allocations: 3.111 MiB)
   # dense  J 0.022079 seconds (283.54 k allocations: 18.146 MiB)
   
+function getInitOrderWavefront(fg, state_label::Symbol=:parametric; depth::Int=1)
+    cliques = NamedTuple{(:frontals, :separators), Tuple{Vector{Symbol}, Vector{Symbol}}}[]
+    
+    all_vls = listVariables(fg)
+    knowns = filter(vl -> isInitialized(fg, vl, state_label), all_vls)
+    unknowns = setdiff(all_vls, knowns)
+    
+    prior_vls, _ = listNeighborhood(fg, lsfPriors(fg), 1)
+    
+    while !isempty(unknowns)
+        anchors = union(knowns, prior_vls)
+        
+        # Find Frontals
+        neighborhood_vls, _ = isempty(anchors) ? (Symbol[], Symbol[]) : listNeighborhood(fg, anchors, 2 * depth)
+        frontals = intersect(neighborhood_vls, unknowns)
+        
+        # Safety break if graph is completely floating (no priors, no knowns left to expand from)
+        isempty(frontals) && break
+        
+        # Find Separators (Initialized variables touching our new frontals)
+        frontal_neighbors, _ = listNeighborhood(fg, frontals, 2)
+        separators = intersect(frontal_neighbors, knowns)
+        
+        # Record and Advance
+        push!(cliques, (; frontals, separators))
+        union!(knowns, frontals)
+        setdiff!(unknowns, frontals)
+    end
+    
+    return cliques
+end
+
 function autoinitParametric!(
   fg,
-  clique_order = getInitOrderParametric(fg);
+  clique_order = getInitOrderWavefront(fg; depth=3);
   reinit = false,
   kwargs...
 )
-  init_labels = @showprogress map(clique_order) do cliq
-    autoinitParametric!(fg, cliq.frontals, cliq.separators; reinit, kwargs...)
+  did_init = false
+  @showprogress for cliq in clique_order
+    did_init |= autoinitParametric!(fg, cliq.frontals, cliq.separators; reinit, kwargs...)
   end
-  filter!(!isnothing, init_labels)
-  return init_labels
+
+  return did_init
 end
 
 function autoinitParametric!(dfg::AbstractDFG, initme::Symbol; kwargs...)
@@ -766,6 +799,11 @@ function autoinitParametric!(
   filter!(fl -> issubset(getVariableOrder(dfg, fl), varlabels), faclabels)
   isempty(faclabels) && return false
 
+  # Prune stranded frontals
+  connected_vars = unique(Iterators.flatten(getVariableOrder.(dfg, faclabels)))
+  filter!(v -> v in connected_vars, to_init)
+  isempty(to_init) && return false
+
   # Seed each frontal from an initialized separator of the same type
   for v in to_init
     xi = getVariable(dfg, v)
@@ -780,6 +818,12 @@ function autoinitParametric!(
         DFG.refMeans(vnd)[1] = DFG.refMeans(getState(dfg, same_kind[1], solveKey))[1]
       end
     end
+    
+    # perturb point slightly
+    _M = getManifold(xi)
+    tangent_coords = randn(manifold_dimension(_M)) * 1e-3
+    X = get_vector(LieAlgebra(_M), tangent_coords)
+    DFG.refMeans(vnd)[1] = exp(_M, DFG.refMeans(vnd)[1], X)
   end
 
   # Solve

From 26f718c07ccfc94da746b7f3d700086918dd41e9 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <6612981+Affie@users.noreply.github.com>
Date: Thu, 23 Jul 2026 15:28:49 +0200
Subject: [PATCH 3/6] Switch default linear_subsolver! to default_lm_lin_solve!
 and fix perturb bug (#1952)

Co-authored-by: Johannes Terblanche <Affie@users.noreply.github.com>
---
 .../parametric/services/ParametricManopt.jl    | 18 ++++++++++--------
 IncrementalInference/src/services/FGOSUtils.jl |  2 +-
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/IncrementalInference/src/parametric/services/ParametricManopt.jl b/IncrementalInference/src/parametric/services/ParametricManopt.jl
index 4d75b1a6..0c83b344 100644
--- a/IncrementalInference/src/parametric/services/ParametricManopt.jl
+++ b/IncrementalInference/src/parametric/services/ParametricManopt.jl
@@ -400,6 +400,7 @@ end
 
 Manopt `DebugAction` that prints the tension (reduced chi-squared) at each iteration.
 Tension = 2*cost / dof, where `dof = N - M` (residual dimension minus state dimension).
+Ideal value is 1.0, with values >> 1.0 indicating underfitting and < 1.0 indicating overfitting.
 
 Usage in `solve_RLM`:
 ```julia
@@ -436,8 +437,7 @@ function solve_RLM(
   finiteDiffCovariance = false,
   jacobian_method::Symbol = :finitediff,
   solveKey::Symbol = :parametric,
-  # linear_subsolver! = Manopt.default_lm_lin_solve!,
-  linear_subsolver! = qr_linear_subsolver!,
+  linear_subsolver! = Manopt.default_lm_lin_solve!,
   kwargs...
 )
 
@@ -575,7 +575,7 @@ function solve_RLM_conditional(
   finiteDiffCovariance=true,
   jacobian_method::Symbol = :finitediff,
   solveKey::Symbol = :parametric,
-  linear_subsolver! = qr_linear_subsolver!,
+  linear_subsolver! = Manopt.default_lm_lin_solve!,
   kwargs...
 )
   is_sparse && error("Sparse solve_RLM_conditional not supported yet")
@@ -769,7 +769,7 @@ function autoinitParametric!(
   separators::Vector{Symbol} = Symbol[];
   solveKey = :parametric,
   reinit::Bool = false,
-  linear_subsolver! = pinv_subsolver!,
+  linear_subsolver! = Manopt.default_lm_lin_solve!,
   kwargs...,
 )
   # Filter to only uninitialized variables (unless reinit)
@@ -808,8 +808,9 @@ function autoinitParametric!(
   for v in to_init
     xi = getVariable(dfg, v)
     vnd = getState(xi, solveKey)
-    has_prior = any(isPrior.(dfg, listNeighbors(dfg, v)))
-    if !has_prior && !isempty(active_separators)
+    # has_prior = any(isPrior.(dfg, listNeighbors(dfg, v)))
+    # if !has_prior && !isempty(active_separators)
+    if !isempty(active_separators)
       my_kind = getStateKind(xi)
       same_kind = filter(active_separators) do vl
         getStateKind(getVariable(dfg, vl)) === my_kind
@@ -822,8 +823,9 @@ function autoinitParametric!(
     # perturb point slightly
     _M = getManifold(xi)
     tangent_coords = randn(manifold_dimension(_M)) * 1e-3
-    X = get_vector(LieAlgebra(_M), tangent_coords)
-    DFG.refMeans(vnd)[1] = exp(_M, DFG.refMeans(vnd)[1], X)
+    mn = DFG.refMeans(vnd)[1]
+    X = hat(LieAlgebra(_M), tangent_coords, typeof(mn))
+    DFG.refMeans(vnd)[1] = exp(_M, mn, X)
   end
 
   # Solve
diff --git a/IncrementalInference/src/services/FGOSUtils.jl b/IncrementalInference/src/services/FGOSUtils.jl
index 9b40a431..64285115 100644
--- a/IncrementalInference/src/services/FGOSUtils.jl
+++ b/IncrementalInference/src/services/FGOSUtils.jl
@@ -71,7 +71,7 @@ _getZDim(ccw::CommonConvWrapper) = getManifold(ccw) |> manifold_dimension # ccw.
 # TODO is MsgPrior piggy backing zdim on inferdim???
 _getZDim(ccw::CommonConvWrapper{<:MsgPrior}) = length(ccw.usrfnc!.infoPerCoord) # ccw.usrfnc!.inferdim
 
-_getZDim(fct::FactorCompute) = _getCCW(fct) |> _getZDim
+_getZDim(fct::FactorCompute) = getManifold(fct) |> manifold_dimension
 
 DFG.getDimension(fct::FactorCompute) = _getZDim(fct)
 

From a171c90ab7cfff16ec441968bf5460a7fd4781e7 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <Affie@users.noreply.github.com>
Date: Fri, 3 Nov 2023 08:20:22 +0200
Subject: [PATCH 4/6] wip parametric tree solve

---
 .../services/CliqueStateMachine.jl            |   2 +-
 .../src/Factors/GenericFunctions.jl           |  76 +++++++
 .../services/ParametricCSMFunctions.jl        | 194 +++++++++++++++++-
 .../src/services/TreeMessageUtils.jl          |  22 ++
 4 files changed, 282 insertions(+), 12 deletions(-)

diff --git a/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl b/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl
index d070486c..5bfe90ce 100644
--- a/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl
+++ b/IncrementalInference/src/CliqueStateMachine/services/CliqueStateMachine.jl
@@ -956,7 +956,7 @@ function updateFromSubgraph_StateMachine(csmc::CliqStateMachineContainer)
   logCSM(
     csmc,
     "CSM-5 Clique $(csmc.cliq.id) finished, solveKey=$(csmc.solveKey)";
-    loglevel = Logging.Info,
+    loglevel = Logging.Debug,
   )
   return IncrementalInference.exitStateMachine
 end
diff --git a/IncrementalInference/src/Factors/GenericFunctions.jl b/IncrementalInference/src/Factors/GenericFunctions.jl
index b8a2e69d..7cd59527 100644
--- a/IncrementalInference/src/Factors/GenericFunctions.jl
+++ b/IncrementalInference/src/Factors/GenericFunctions.jl
@@ -94,6 +94,82 @@ function (cf::CalcFactor{<:ManifoldFactor})(X, p, q)
   return measurement_residual(cf.factor.M, X, p, q)
 end
 
+## ======================================================================================
+## adjoint factor - adjoint action applied to the measurement
+## ======================================================================================
+function Ad(::Union{typeof(SpecialEuclidean(2)), typeof(SpecialEuclidean(3))}, p, X)
+  t = p.x[1]
+  R = p.x[2]
+  v = X.x[1]
+  Ω = X.x[2]
+  ArrayPartition(-R*Ω*R'*t + R*v, R*Ω*R')
+end
+
+function Ad(::typeof(SpecialEuclidean(3)), p)
+  t = p.x[1]
+  R = p.x[2]
+  vcat(
+      hcat(R, skew(t)*R),
+      hcat(zero(SMatrix{3,3,Float64}), R)
+  )
+end
+
+function Ad(::typeof(SpecialEuclidean(2)), p)
+  t = p.x[1]
+  R = p.x[2]
+  vcat(
+      hcat(R, -SA[0 -1; 1 0]*t),
+      SA[0 0 1]
+  )
+end
+
+struct AdFactor{F <: AbstractManifoldMinimize} <: AbstractManifoldMinimize
+  factor::F
+end
+
+function (cf::CalcFactor{<:AdFactor})(Xϵ, p, q)
+  # M = getManifold(cf.factor)
+  # p,q ∈ M
+  # Xϵ ∈ TϵM
+  # ϵ = identity_element(M)
+  # transform measurement from TϵM to TpM (global to local coordinates)
+  # Adₚ⁻¹ = AdjointMatrix(M, p)⁻¹ = AdjointMatrix(M, p⁻¹)
+  # Xp = Adₚ⁻¹ * Xϵᵛ
+  # ad = Ad(M, inv(M, p))
+  # Xp = Ad(M, inv(M, p), Xϵ)
+  # Xp = adjoint_action(M, inv(M, p), Xϵ)
+  #TODO is vector transport supposed to be the same?
+  # Xp = vector_transport_to(M, ϵ, Xϵ, p)
+
+  # Transform measurement covariance
+  # ᵉΣₚ = Adₚ ᵖΣₚ Adₚᵀ
+  #TODO test if transforming sqrt_iΣ is the same as Σ
+  # Σ = ad * inv(cf.sqrt_iΣ^2) * ad'
+  # sqrt_iΣ = convert(typeof(cf.sqrt_iΣ), sqrt(inv(Σ)))
+  # sqrt_iΣ = convert(typeof(cf.sqrt_iΣ), ad * cf.sqrt_iΣ * ad')
+  Xp = Xϵ
+
+  child_cf = CalcFactorResidual(
+    cf.faclbl,
+    cf.factor.factor,
+    cf.varOrder,
+    cf.varOrderIdxs,
+    cf.meas,
+    cf.sqrt_iΣ,
+    cf.cache,
+  )
+  return child_cf(Xp, p, q)
+end
+
+getMeasurementParametric(f::AdFactor) = getMeasurementParametric(f.factor)
+
+getManifold(f::AdFactor) = getManifold(f.factor)
+function getSample(cf::CalcFactor{<:AdFactor}) 
+  M = getManifold(cf)
+  return sampleTangent(M, cf.factor.factor.Z)
+end
+
+
 ## ======================================================================================
 ## adjoint factor - adjoint action applied to the measurement
 ## ======================================================================================
diff --git a/IncrementalInference/src/parametric/services/ParametricCSMFunctions.jl b/IncrementalInference/src/parametric/services/ParametricCSMFunctions.jl
index 16c692eb..73a24135 100644
--- a/IncrementalInference/src/parametric/services/ParametricCSMFunctions.jl
+++ b/IncrementalInference/src/parametric/services/ParametricCSMFunctions.jl
@@ -5,7 +5,7 @@
 Notes
 - Parametric state machine function nr. 3
 """
-function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
+function solveUp_ParametricStateMachine_Old(csmc::CliqStateMachineContainer)
   infocsm(csmc, "Par-3, Solving Up")
 
   setCliqueDrawColor!(csmc.cliq, "red")
@@ -96,6 +96,145 @@ function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
   return waitForDown_StateMachine
 end
 
+# solve relatives ignoring any priors keeping `from` at ϵ
+# if clique has priors : solve to get a prior on `from`
+# send messages as factors or just the beliefs? for now factors
+function solveUp_ParametricStateMachine(csmc::CliqStateMachineContainer)
+  infocsm(csmc, "Par-3, Solving Up")
+
+  setCliqueDrawColor!(csmc.cliq, "red")
+  # csmc.drawtree ? drawTree(csmc.tree, show=false, filepath=joinpath(getSolverParams(csmc.dfg).logpath,"bt.pdf")) : nothing
+
+  msgfcts = Symbol[]
+
+  for (idx, upmsg) in getMessageBuffer(csmc.cliq).upRx #get cached messages taken from children saved in this clique
+    child_factors = addMsgFactors_Parametric!(csmc.cliqSubFg, upmsg, UpwardPass)
+    append!(msgfcts, getLabel.(child_factors)) # addMsgFactors_Parametric!
+  end
+  logCSM(csmc, "length mgsfcts=$(length(msgfcts))")
+  infocsm(csmc, "length mgsfcts=$(length(msgfcts))")
+
+  # store the cliqSubFg for later debugging
+  _dbgCSMSaveSubFG(csmc, "fg_beforeupsolve")
+
+  subfg = csmc.cliqSubFg
+
+  frontals = getCliqFrontalVarIds(csmc.cliq)
+  separators = getCliqSeparatorVarIds(csmc.cliq)
+
+  # if its a root do full solve
+  if length(getParent(csmc.tree, csmc.cliq)) == 0
+    # M, vartypeslist, lm_r, Σ = solve_RLM(subfg; is_sparse=false, finiteDiffCovariance=true)
+    autoinitParametric!(subfg)
+    M, vartypeslist, lm_r, Σ = solveGraphParametric!(subfg; is_sparse=false, finiteDiffCovariance=true, damping_term_min=1e-18)
+
+  else
+
+    # select first seperator as constant reference at the identity element
+    isempty(separators) && @warn "empty separators solving cliq $(csmc.cliq.id.value)" ls(subfg) lsf(subfg)
+    from = first(separators)
+    from_v = getVariable(subfg, from)
+    getSolverData(from_v, :parametric).val[1] = getPointIdentity(getVariableType(from_v))
+
+    #TODO handle priors
+    # Variables that are free to move
+    free_vars = [frontals; separators[2:end]]
+    # Solve for the free variables
+
+    @assert !isempty(lsf(subfg)) "No factors in clique $(csmc.cliq.id.value) ls=$(ls(subfg)) lsf=$(lsf(subfg))"
+
+    # M, vartypeslist, lm_r, Σ = solve_RLM_conditional(subfg, free_vars, [from];)
+    M, vartypeslist, lm_r, Σ = solve_RLM_conditional(subfg, free_vars, [from]; finiteDiffCovariance=false, damping_term_min=1e-18)
+
+  end
+  
+  # FIXME check solve convergence
+  if !true
+    @error "Par-3, clique $(csmc.cliq.id) failed to converge in upsolve" result
+    # propagate error to cleanly exit all cliques
+    putErrorUp(csmc)
+    if length(getParent(csmc.tree, csmc.cliq)) == 0
+      putErrorDown(csmc)
+      return IncrementalInference.exitStateMachine
+    end
+
+    return waitForDown_StateMachine
+  end
+
+  logCSM(csmc, "$(csmc.cliq.id): subfg solve converged sending messages")
+  
+  # Pack results in massage factors
+  
+  sigmas = extractMarginalsAP(M, vartypeslist, Σ)
+  
+  # FIXME fix MsgRelativeType
+  relative_message_factors = MsgRelativeType();
+  for (i, to) in enumerate(vartypeslist)
+    if to in separators
+      #assume full dim factor
+      factype = selectFactorType(subfg, from, to)
+      # make S symetrical
+      # S = sigmas[i] # FIXME for some reason SMatrix is not invertable even though it is!!!!!!!!
+      S = Matrix(sigmas[i])# FIXME
+      S = (S + S') / 2
+      # @assert all(isapprox.(S, sigmas[i], rtol=1e-3)) "Bad covariance matrix - not symetrical"
+      !all(isapprox.(S, sigmas[i], rtol=1e-3)) && @error("Bad covariance matrix - not symetrical")
+      # @assert all(diag(S) .> 0) "Bad covariance matrix - not positive diag"
+      !all(diag(S) .> 0) && @error("Bad covariance matrix - not positive diag")
+
+      
+      M_to = getManifold(getVariableType(subfg, to))
+      ϵ = getPointIdentity(M_to)
+      μ = vee(M_to, ϵ, log(M_to, ϵ, lm_r[i]))
+        
+      message_factor = AdFactor(factype(MvNormal(μ, S)))
+      
+     
+      # logCSM(csmc, "$(csmc.cliq.id): Z=$(getMeasurementParametric(message_factor))"; loglevel = Logging.Warn)
+
+      push!(relative_message_factors, (variables=[from, to], likelihood=message_factor))
+    end
+  end
+
+  # Done with solve delete factors
+  #TODO confirm, maybe don't delete mesage factors on subgraph, maybe delete if its priors, but not conditionals
+  # deleteMsgFactors!(csmc.cliqSubFg)
+
+  # store the cliqSubFg for later debugging
+  _dbgCSMSaveSubFG(csmc, "fg_afterupsolve")
+
+  # cliqueLikelihood = calculateMarginalCliqueLikelihood(vardict, Σ, varIds, cliqSeparatorVarIds)
+
+  #Fill in CliqueLikelihood
+  beliefMsg = LikelihoodMessage(;
+    sender = (; id = csmc.cliq.id.value, step = csmc._csm_iter),
+    status = UPSOLVED,
+    variableOrder = separators,
+    # cliqueLikelihood,
+    jointmsg = _MsgJointLikelihood(;relatives=relative_message_factors),
+    msgType = ParametricMessage(),
+  )
+
+  # @assert length(separators) <= 2 "TODO length(separators) = $(length(separators)) > 2 in clique $(csmc.cliq.id.value)"
+  @assert isempty(lsfPriors(csmc.cliqSubFg)) || csmc.cliq.id.value == 1 "TODO priors in clique $(csmc.cliq.id.value)"
+  # if length(lsfPriors(csmc.cliqSubFg)) > 0 || length(separators) > 2
+    # for si in cliqSeparatorVarIds
+    #   vnd = getSolverData(getVariable(csmc.cliqSubFg, si), :parametric)
+    #   beliefMsg.belief[si] = TreeBelief(deepcopy(vnd))
+    # end
+  # end
+
+  for e in getEdgesParent(csmc.tree, csmc.cliq)
+    logCSM(csmc, "$(csmc.cliq.id): put! on edge $(e)")
+    getMessageBuffer(csmc.cliq).upTx = deepcopy(beliefMsg)
+    putBeliefMessageUp!(csmc.tree, e, beliefMsg)
+  end
+
+  return waitForDown_StateMachine
+end
+
+global g_n = nothing
+
 """
     $SIGNATURES
 
@@ -120,6 +259,14 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
         logCSM(csmc, "$(csmc.cliq.id): Updating separator $msym from message $(belief.val)")
         DFG.refMeans(vnd)[1] = belief.val[1] #FIXME 🦨 shares data structure in belief
         DFG.refCovariances(vnd)[1] = belief.bw
+        p = belief.val[1]
+
+        S = belief.bw
+        S = (S + S') / 2
+        # vnd.bw .= S
+        
+        nd = MvNormal(getCoordinates(Main.Pose2, p), S)
+        addFactor!(csmc.cliqSubFg, [msym], Main.PriorPose2(nd))
       end
     end
   end
@@ -132,23 +279,48 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
   #only down solve if its not a root
   if length(getParent(csmc.tree, csmc.cliq)) != 0
     frontals = getCliqFrontalVarIds(csmc.cliq)
-    vardict, result, flatvars, Σ = solveConditionalsParametric(csmc.cliqSubFg, frontals)
+    # vardict, result, flatvars, Σ = solveConditionalsParametric(csmc.cliqSubFg, frontals)
     #TEMP testing difference
     # vardict, result = solveGraphParametric(csmc.cliqSubFg)
     # Pack all results in variables
-    if Optim.g_converged(result) || Optim.f_converged(result)
+    @assert !isempty(lsf(csmc.cliqSubFg)) "No factors in clique $(csmc.cliq.id.value) ls=$(ls(csmc.cliqSubFg)) lsf=$(lsf(csmc.cliqSubFg))"
+
+    # M, vartypeslist, lm_r, Σ = solve_RLM_conditional(csmc.cliqSubFg, frontals; finiteDiffCovariance=false, damping_term_min=1e-18)
+    M, vartypeslist, lm_r, Σ = solve_RLM(csmc.cliqSubFg; finiteDiffCovariance=false, damping_term_min=1e-18)
+    sigmas = extractMarginalsAP(M, vartypeslist, Σ)
+
+    if true # TODO check for convergence result.g_converged || result.f_converged
       logCSM(
         csmc,
         "$(csmc.cliq.id): subfg optim converged updating variables";
-        loglevel = Logging.Info,
+        loglevel = Logging.Debug,
       )
-      for (v, val) in vardict
-        logCSM(csmc, "$(csmc.cliq.id) down: updating $v : $val"; loglevel = Logging.Info)
-        vnd = getState(getVariable(csmc.cliqSubFg, v), :parametric)
-        #Update subfg variables
-        DFG.refMeans(vnd)[1] = val.val
-        DFG.refCovariances(vnd)[1] = val.cov
+      for (i, v) in enumerate(vartypeslist)
+        if v in frontals
+          # logCSM(csmc, "$(csmc.cliq.id) down: updating $v"; val, loglevel = Logging.Debug)
+          vnd = getState(getVariable(csmc.cliqSubFg, v), :parametric)
+          
+          S = Matrix(sigmas[i])# FIXME
+          S = (S + S') / 2
+          # @assert all(isapprox.(S, sigmas[i], rtol=1e-3)) "Bad covariance matrix - not symetrical"
+          !all(isapprox.(S, sigmas[i], rtol=1e-3)) && @error("Bad covariance matrix - not symetrical")
+          # @assert all(diag(S) .> 0) "Bad covariance matrix - not positive diag"
+          !all(diag(S) .> 0) && @error("Bad covariance matrix - not positive diag")
+
+          
+          #Update subfg variables
+          DFG.refMeans(vnd)[1] = lm_r[i]
+          DFG.refCovariances(vnd)[1] = S
+        end
       end
+      # for (v, val) in vardict
+      #   logCSM(csmc, "$(csmc.cliq.id) down: updating $v"; val, loglevel = Logging.Debug)
+      #   vnd = getSolverData(getVariable(csmc.cliqSubFg, v), :parametric)
+        
+      #   #Update subfg variables
+      #   vnd.val[1] = val.val
+      #   vnd.bw .= val.cov
+      # end
     else
       @error "Par-5, clique $(csmc.cliq.id) failed to converge in down solve" result
       #propagate error to cleanly exit all cliques
@@ -169,7 +341,7 @@ function solveDown_ParametricStateMachine(csmc::CliqStateMachineContainer)
   for fi in cliqFrontalVarIds
     vnd = getState(getVariable(csmc.cliqSubFg, fi), :parametric)
     beliefMsg.belief[fi] = TreeBelief(vnd)
-    logCSM(csmc, "$(csmc.cliq.id): down message $fi : $beliefMsg"; loglevel = Logging.Info)
+    logCSM(csmc, "$(csmc.cliq.id): down message $fi"; beliefMsg=beliefMsg.belief[fi], loglevel = Logging.Debug)
   end
 
   # pass through the frontal variables that were sent from above
diff --git a/IncrementalInference/src/services/TreeMessageUtils.jl b/IncrementalInference/src/services/TreeMessageUtils.jl
index 4aceffce..fcd92646 100644
--- a/IncrementalInference/src/services/TreeMessageUtils.jl
+++ b/IncrementalInference/src/services/TreeMessageUtils.jl
@@ -573,6 +573,28 @@ function addMsgFactors!(
   return msgfcts
 end
 
+function addMsgFactors_Parametric!(
+  subfg::AbstractDFG,
+  msg::LikelihoodMessage,
+  ::Type{UpwardPass};
+  tags::Vector{Symbol} = Symbol[],
+  # attemptPriors::Bool = true,
+)
+  # add differential(relative) message factors
+
+  msgfcts = map(msg.jointmsg.relatives) do difflikl
+    addFactor!(
+      subfg,
+      difflikl.variables,
+      difflikl.likelihood;
+      graphinit = false,
+      tags = union(tags, [:__LIKELIHOODMESSAGE__; :__UPWARD_DIFFERENTIAL__]),
+    )
+  end
+
+  return msgfcts
+end
+
 function addMsgFactors!(
   subfg::AbstractDFG,
   allmsgs::Dict{Int, LikelihoodMessage},

From 0981992822fcdf3b502a1045cee771c05dc16dc0 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <Affie@users.noreply.github.com>
Date: Thu, 3 Jul 2025 13:23:40 +0200
Subject: [PATCH 5/6] stash on par_tree

---
 IncrementalInference/src/Factors/GenericFunctions.jl | 9 +++++++++
 IncrementalInference/src/services/FactorGradients.jl | 2 +-
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/IncrementalInference/src/Factors/GenericFunctions.jl b/IncrementalInference/src/Factors/GenericFunctions.jl
index 7cd59527..2df74acf 100644
--- a/IncrementalInference/src/Factors/GenericFunctions.jl
+++ b/IncrementalInference/src/Factors/GenericFunctions.jl
@@ -123,6 +123,15 @@ function Ad(::typeof(SpecialEuclidean(2)), p)
   )
 end
 
+function Ad(::Motion(2), p)
+  t = p.x[1]
+  R = p.x[2]
+  vcat(
+      hcat(R, -SA[0 -1; 1 0]*t),
+      SA[0 0 1]
+  )
+end
+
 struct AdFactor{F <: AbstractManifoldMinimize} <: AbstractManifoldMinimize
   factor::F
 end
diff --git a/IncrementalInference/src/services/FactorGradients.jl b/IncrementalInference/src/services/FactorGradients.jl
index e1831530..3ef89d43 100644
--- a/IncrementalInference/src/services/FactorGradients.jl
+++ b/IncrementalInference/src/services/FactorGradients.jl
@@ -31,7 +31,7 @@ function factorJacobian(
   M_codom = Euclidean(manifold_dimension(getManifold(fac)))
   # Jx(M, p) = ManifoldDiff.jacobian(M, M_codom, calcfac, p, backend)
   
-  return ManifoldDiff.jacobian(M_dom, M_codom, costf, p0, backend)
+  return ManifoldDiff.jacobian(M_dom, M_codom, costf, p0, backend), costf(p0)
 end
 
 

From 8915a69b951c3edac026ec733f2961f6f61c9cf7 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <Affie@users.noreply.github.com>
Date: Thu, 3 Jul 2025 13:35:23 +0200
Subject: [PATCH 6/6] unpackDistribution fixes covar

---
 .../services/SerializingDistributions.jl        | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)

diff --git a/IncrementalInferenceTypes/src/serialization/services/SerializingDistributions.jl b/IncrementalInferenceTypes/src/serialization/services/SerializingDistributions.jl
index 4e6ac01d..82d28913 100644
--- a/IncrementalInferenceTypes/src/serialization/services/SerializingDistributions.jl
+++ b/IncrementalInferenceTypes/src/serialization/services/SerializingDistributions.jl
@@ -28,3 +28,20 @@ function DFG.unpack(dtr::PackedFullNormal)
 end
 DFG.unpack(dtr::PackedRayleigh) = Rayleigh(dtr.sigma)
 
+
+# function unpackDistribution(dtr::PackedFullNormal)
+  
+#   SM = SymmetricPositiveDefinite(length(dtr.mu))
+#   S = reshape(dtr.cov, length(dtr.mu), :)
+#   ch = check_point(SM, S; atol = 1e-9) 
+#   if !isnothing(ch)
+#       @warn "IMU Covar check" ch
+#       S = (S + S') / 2
+#       S = S + diagm((diag(S) .== 0)*1e-15)
+#       ch = check_point(SM, S)
+#       !isnothing(ch) && @error "IMU Covar check" ch
+#   end
+
+#   # return MvNormal(dtr.mu, reshape(dtr.cov, length(dtr.mu), :))
+#   return MvNormal(dtr.mu, S)
+# end
\ No newline at end of file