JuliaRobotics · dehann · Aug 6, 2026 · Aug 10, 2026 · Aug 11, 2026
diff --git a/Project.toml b/Project.toml
@@ -35,6 +35,7 @@ JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6"
 JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3"
 JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
+LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377"
@@ -46,6 +47,7 @@ Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f"
 PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a"
 Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7"
 ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca"
+RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
@@ -58,7 +60,6 @@ TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
-Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02"
 YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6"
 
 [weakdeps]
@@ -78,56 +79,57 @@ CaesarLasIOExt = "LasIO"
 CaesarZMQExt = "ZMQ"
 
 [compat]
-ApproxManifoldProducts = "0.7, 0.8"
+ApproxManifoldProducts = "0.15"
 AprilTags = "0.8, 0.9"
 Clustering = "0.15"
-ColorVectorSpace = "0.9, 0.10"
-Colors = "0.12"
+ColorVectorSpace = "0.11"
+Colors = "0.13"
 Combinatorics = "1"
 CoordinateTransformations = "0.5, 0.6"
 DataStructures = "0.17, 0.18"
 DelimitedFiles = "1"
 Distances = "0.10"
-DistributedFactorGraphs = "0.22, 0.23, 0.24"
+DistributedFactorGraphs = "0.24 - 0.29"
 Distributions = "0.25"
 DocStringExtensions = "0.8, 0.9"
 FFTW = "1"
 FileIO = "1"
 FixedPointNumbers = "0.8"
 GeometricalPredicates = "0.4"
-GeometryBasics = "0.4"
+GeometryBasics = "0.4, 0.5"
 Graphs = "1"
 ImageCore = "0.8, 0.9, 0.10"
 ImageDraw = "0.2"
 ImageFeatures = "0.5"
 ImageMagick = "1"
 Images = "0.26"
-IncrementalInference = "0.34, 0.35"
+IncrementalInference = "0.35 - 0.38"
 Interpolations = "0.14, 0.15"
-JLD2 = "0.3, 0.4"
-JSON = "0.20, 0.21"
+JLD2 = "0.6"
+JSON = "1"
 JSON2 = "0.3, 0.4"
 JSON3 = "1"
 KernelDensityEstimate = "0.5"
 LasIO = "0.3"
-Manifolds = "0.8, 0.9"
+LieGroups = "0.1"
+Manifolds = "0.9, 0.10, 0.11"
 MetaGraphsNext = "0.7"
 MultivariateStats = "0.10"
 NLsolve = "4"
 NearestNeighbors = "0.4"
 Optim = "1"
 PrecompileTools = "1"
 ProgressMeter = "1"
+RecursiveArrayTools = "3.27, 4"
 Reexport = "1"
-RoME = "0.23, 0.24"
+RoME = "0.24, 0.25, 0.26"
 Rotations = "1.1"
 StaticArrays = "1"
 Statistics = "1"
 StatsBase = "0.33, 0.34"
 TensorCast = "0.4"
 TimeZones = "1.3.1, 1.4"
 TransformUtils = "0.2.14"
-Unmarshal = "0.3, 0.4"
 YAML = "0.3, 0.4"
 ZMQ = "1"
 julia = "1.9"

diff --git a/ext/CaesarImageMagickExt.jl b/ext/CaesarImageMagickExt.jl
@@ -56,38 +56,57 @@ end
 #   # ImageMagick.readblob(imgBytes)
 # end
 
-"""
-    $SIGNATURES
-
-`Data: Entry => Blob` helper function to load images stored in standard (png, jpg, jpeg) format from supported a DFG data blob store.
-
-Example
-```julia
-# Skip if AbstractBlobStore is already set up
-  ## if a FolderStore already exists (assumed inside `getLogPath(fg)`)
-  storeDir = joinLogPath(fg,"data")
-  datastore = FolderStore{Vector{UInt8}}(:default_folder_store, storeDir) 
-  addBlobStore!(fg, datastore)
-
-# Fetch the image
-img = fetchDataImage(fg, :x4, :KEYFRAME_IMG)
-```
 
-Notes
-- https://juliarobotics.org/Caesar.jl/latest/concepts/interacting_fgs/#Retrieving-a-Data-Blob
-"""
-function fetchDataImage(dfg::AbstractDFG,
-                        varLbl::Symbol,
-                        dataLbl::Symbol,
-                        getDataLambda::Function = (g,vl,dl) -> getData(g,vl,dl),
-                        checkMimeType::Bool=true )
-#
-  imgEntry, imgBytes = getDataLambda(dfg, varLbl, dataLbl)
-  allowList = ["image/png"; "image/jpg"; "image/jpeg"]
-  checkMimeType && (@assert imgEntry.mimeType in allowList "Should we allow image format DataBlobEntry.mimeType=$(imgEntry.mimeType), current allowList=$allowList")
-  ImageMagick.readblob(imgBytes)
+function fetchDataImage(w...)
+  error("fetchDataImage is obsolete, use DistributedFactorGraphs.loadImage_Variable(dfg::AbstractDFG, variable_label::Symbol, entry_label::Symbol) instead")
 end
-fetchDataImage(dfg::AbstractDFG,datastore::AbstractBlobStore,varLbl::Symbol,dataLbl::Symbol,checkMimeType::Bool=true) = fetchDataImage(dfg, varLbl, dataLbl, (g,vl,dl) -> getData(g,datastore,vl,dl) , checkMimeType)
+# """
+#     $SIGNATURES
+
+# `Data: Entry => Blob` helper function to load images stored in standard (png, jpg, jpeg) format from supported a DFG data blob store.
+
+# Example
+# ```julia
+# # Skip if AbstractBlobStore is already set up
+#   ## if a FolderStore already exists (assumed inside `getLogPath(fg)`)
+#   storeDir = joinLogPath(fg,"data")
+#   datastore = FolderStore{Vector{UInt8}}(:default_folder_store, storeDir) 
+#   addBlobStore!(fg, datastore)
+
+# # Fetch the image
+# img = fetchDataImage(fg, :x4, :KEYFRAME_IMG)
+# ```
+
+# Notes
+# - https://juliarobotics.org/Caesar.jl/latest/concepts/interacting_fgs/#Retrieving-a-Data-Blob
+# """
+# function fetchDataImage(
+#   dfg::AbstractDFG,
+#   varLbl::Symbol,
+#   dataLbl::Symbol,
+#   getDataLambda::Function = (g,vl,dl) -> getData(g,vl,dl),
+#   checkMimeType::Bool=true 
+# )
+# #
+#   imgEntry, imgBytes = getDataLambda(dfg, varLbl, dataLbl)
+#   allowList = ["image/png"; "image/jpg"; "image/jpeg"]
+#   checkMimeType && (@assert imgEntry.mimeType in allowList "Should we allow image format DataBlobEntry.mimeType=$(imgEntry.mimeType), current allowList=$allowList")
+#   ImageMagick.readblob(imgBytes)
+# end
+# fetchDataImage(
+#   dfg::AbstractDFG,
+#   datastore::AbstractBlobStore,
+#   varLbl::Symbol,
+#   dataLbl::Symbol,
+#   checkMimeType::Bool=true
+# ) = 
+#   fetchDataImage(
+#     dfg, 
+#     varLbl, 
+#     dataLbl, 
+#     (g,vl,dl) -> getData(g,datastore,vl,dl), 
+#     checkMimeType
+#   )
 
 
 

diff --git a/ext/CaesarImagesExt.jl b/ext/CaesarImagesExt.jl
@@ -7,6 +7,7 @@ using UUIDs
 using TensorCast
 using StaticArrays
 using Manifolds
+using LieGroups
 using DocStringExtensions
 using ProgressMeter
 using Optim

diff --git a/ext/CaesarZMQExt.jl b/ext/CaesarZMQExt.jl
@@ -3,7 +3,7 @@ module CaesarZMQExt
 # using Requires
 using JSON
 using Distributions, IncrementalInference, DistributedFactorGraphs, Caesar
-using Unmarshal
+# using Unmarshal
 using Dates
 using DocStringExtensions
 

diff --git a/ext/Images/ScatterAlignPose2.jl b/ext/Images/ScatterAlignPose2.jl
@@ -37,8 +37,8 @@ function ScatterAlignPose2(
 end
 
 function ScatterAlignPose2(;
-    cloud1::ManifoldKernelDensity, 
-    cloud2::ManifoldKernelDensity,
+    cloud1::HomotopyDensity, 
+    cloud2::HomotopyDensity,
     sample_count::Integer=75,
     bw::Real=5e-5, # from a sensitivity analysis with marine radar data (50 or 100 samples)
     rescale::Real=1,
@@ -64,8 +64,8 @@ function ScatterAlignPose2(;
 end
 
 function ScatterAlignPose3(;
-    cloud1::ManifoldKernelDensity, 
-    cloud2::ManifoldKernelDensity,
+    cloud1::HomotopyDensity, 
+    cloud2::HomotopyDensity,
     sample_count::Integer=75,
     bw::Real=5e-5, # from a sensitivity analysis with marine radar data (50 or 100 samples)
     rescale::Real=1,
@@ -91,7 +91,7 @@ function ScatterAlignPose3(;
 end
 
 function ScatterAlignPose3(
-  ::Type{<:ManifoldKernelDensity};
+  ::Type{<:HomotopyDensity};
   cloud1::_PCL.PointCloud,
   cloud2::_PCL.PointCloud,
   bw1 = [1;1;1.0],
@@ -114,7 +114,7 @@ getManifold(::IIF.InstanceType{<:ScatterAlignPose3}) = getManifold(Pose3Pose3)
 # runs once upon addFactor! and returns object later used as `cache`
 function preambleCache(
   dfg::AbstractDFG, 
-  vars::AbstractVector{<:DFGVariable}, 
+  vars::AbstractVector{<:VariableCompute}, 
   fnc::Union{<:ScatterAlignPose2,<:ScatterAlignPose3}
 )
   #
@@ -126,10 +126,10 @@ function preambleCache(
     if fnc.align.useStashing 
       @assert 0 < length(de) "cannot reconstitute ScatterAlignPose2 without necessary data entry, only have $de"
       _, db = getData(dfg, getLabel(va), UUID(de)) # fnc.align.dataStoreHint
-      # Assume PackedManifoldKernelDensity
+      # Assume PackedHomotopyDensity
       cld = convert(SamplableBelief, String(take!(IOBuffer(db))))
       # payload = JSON.parse(String(take!(IOBuffer(db))))
-      # dstr = unmarshal PackedManifoldKernelDensity
+      # dstr = unmarshal PackedHomotopyDensity
       # cld = unpackDistribution(dstr)
       # update either a HGD or MKD
       _update!(cl, cld)
@@ -185,7 +185,7 @@ function getSample( cf::CalcFactor{S} ) where {S <: Union{<:ScatterAlignPose2,<:
     #  TODO relax to Riemannian where e0 is replaced by any point
     return hat(M, e0, res.minimizer)
   else #if cf.factor.align.sample_count < 0
-    @assert cf.factor.align.cloud1 isa ManifoldKernelDensity "ICP alignments currently only implemented for beliefs as MKDs"
+    @assert cf.factor.align.cloud1 isa HomotopyDensity "ICP alignments currently only implemented for beliefs as MKDs"
     ppt = getPoints(cf.factor.align.cloud1)
     qpt_ = getPoints(cf.factor.align.cloud2)
 
@@ -215,19 +215,24 @@ function getSample( cf::CalcFactor{S} ) where {S <: Union{<:ScatterAlignPose2,<:
 end
 
 
-function (cf::CalcFactor{S})(X, p, q
-  ) where {S <: Union{<:ScatterAlignPose2,<:ScatterAlignPose3}}
+function (cf::CalcFactor{S})(
+  X, p, q
+) where {S <: Union{<:ScatterAlignPose2,<:ScatterAlignPose3}}
   # 
 
-  M = cf.cache.M
-  ϵ0 = cf.cache.e0
 
+  M = cf.cache.M
   # copied from Pose2Pose2
-  q̂ = allocate(q)
-  exp!(M, q̂, ϵ0, X)
-  Manifolds.compose!(M, q̂, p, q̂)   
-  Xc = vee(M, q, log!(M, q̂, q, q̂))
-  return Xc
+  # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M
+  X̂ = log(M, p, q)
+  return vee(LieAlgebra(M), X - X̂)
+
+  # ϵ0 = cf.cache.e0
+  # q̂ = allocate(q)
+  # exp!(M, q̂, ϵ0, X)
+  # Manifolds.compose!(M, q̂, p, q̂)   
+  # Xc = vee(M, q, log!(M, q̂, q, q̂))
+  # return Xc
 
     # pXq, 
     # wPp, 
@@ -255,8 +260,7 @@ See also: [`plotScatterAlign`](@ref)
 function overlayScatter(sap::ScatterAlignPose2, 
                         trans::AbstractVector{<:Real}=[0;0.0],
                         rot::Real=0.0;
-                        user_coords = [trans; rot],
-                        # score=Ref(0.0),
+                        user_coords = [trans; rot], # FIXME convert to ArrayPartition
                         sample_count::Integer=sap.align.sample_count,
                         showscore::Bool=true,
                         findBest::Bool=true  )
@@ -400,7 +404,7 @@ function convert(
     end
     nothing
   end
-  _resizeCloudData!(cl::PackedManifoldKernelDensity) = nothing
+  _resizeCloudData!(cl::HomotopyDensityDFG) = nothing
 
   # prep cloud1.data fields for larger data
   if parp.useStashing

diff --git a/ext/factors/Pose2AprilTag4Corners.jl b/ext/factors/Pose2AprilTag4Corners.jl
@@ -77,7 +77,7 @@ end
 
 
 
-Base.@kwdef struct PackedPose2AprilTag4Corners <: AbstractPackedFactor
+Base.@kwdef struct PackedPose2AprilTag4Corners <: AbstractPackedObservation
   # format of serialized data
   _type::String = "Caesar.PackedPose2AprilTag4Corners"
   # corners, as detected by AprilTags library

diff --git a/ext/factors/ScanMatcherPose2.jl b/ext/factors/ScanMatcherPose2.jl
@@ -42,7 +42,7 @@ end
 
 
 
-struct PackedScanMatcherPose2 <: AbstractPackedFactor
+struct PackedScanMatcherPose2 <: AbstractPackedObservation
   im1::Vector{Vector{Float64}}
   im2::Vector{Vector{Float64}}
   gridscale::Float64