Merge pull request #1672 from JuliaRobotics/23Q1/enh/metaprior

add MetaPrior for 0 numeric cases (ie model priors etc)

Author: dehann

NEWS.md

@@ -21,6 +21,7 @@ The list below highlights breaking changes according to normal semver workflow -
 - Refactored, consolidated, and added more in-place operations in surrounding `ccw.measurement`.
 - Refactor `CommonConvWrapper` to a not non-mutable struct, with several cleanups and some updated compat requirements.
 - Refactor interal hard type `HypoRecipe`.
+- Add `MetaPrior` for adding meta data but not influencing the numerical solution.
 
 # Changes in v0.31
 - `FactorMetaData` is deprecated and replaced by `CalcFactor`.

Project.toml

@@ -46,7 +46,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 
 [compat]
-ApproxManifoldProducts = "0.6"
+ApproxManifoldProducts = "0.6.3"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"

src/ExportAPI.jl

@@ -356,4 +356,6 @@ export appendSeparatorToClique!
 export buildTreeFromOrdering! # TODO make internal and deprecate external use to only `buildTreeReset!``
 export makeSolverData!
 
+export MetaPrior
+
 #

src/Factors/MetaPrior.jl

@@ -0,0 +1,12 @@
+# Meta prior brings additional information not necessarily numerical prior
+
+Base.@kwdef struct MetaPrior{T} <: AbstractPrior
+  data::T
+  partial::Vector{Int} = Int[]
+end
+MetaPrior(data) = MetaPrior(;data)
+
+getManifold(::MetaPrior) = TranslationGroup(0)
+getMeasurementParametric(::MetaPrior) = MvNormal(zeros(0))
+
+getSample(cf::CalcFactor{<:MetaPrior}) = SVector()

src/IncrementalInference.jl

@@ -134,6 +134,7 @@ include("VariableStatistics.jl")
 
 # factors needed for belief propagation on the tree
 include("Factors/MsgPrior.jl")
+include("Factors/MetaPrior.jl")
 
 include("entities/CliqueTypes.jl")
 include("entities/JunctionTreeTypes.jl")

src/ParametricUtils.jl

@@ -201,6 +201,8 @@ end
 function calcFactorMahalanobisDict(fg)
   calcFactors = OrderedDict{Symbol, CalcFactorMahalanobis}()
   for fct in getFactors(fg)
+    # skip non-numeric prior
+    getFactorType(fct) isa MetaPrior ? continue : nothing
     calcFactors[fct.label] = CalcFactorMahalanobis(fg, fct)
   end
   return calcFactors

test/testBasicGraphs.jl

@@ -354,7 +354,16 @@ X0reset = pts |> deepcopy
 end
 
 
+@testset "Test MetaPrior" begin
 
+fg = generateGraph_Kaess()
+
+addFactor!(fg, [:x1], MetaPrior(nothing))
+
+solveGraph!(fg)
+IIF.solveGraphParametric!(fg)
+
+end
 
 
 #