Merge pull request #1835 from JuliaRobotics/master

Release v0.35.4
JuliaRobotics · Jul 19, 2024 · 1147e7e · 1147e7e · Affie · Jul 19, 2024
2 parents 89cb345 + 498ab69
commit 1147e7e
Show file tree

Hide file tree

Showing 8 changed files with 110 additions and 82 deletions.
diff --git a/CITATION.bib b/CITATION.bib
@@ -1,7 +1,7 @@
 @online{Caesarjl2021,
  author =        {{P}ackage {C}ontributors and {E}cosystem},
- title =         {Caesar.jl, v0.11.0},
- year =          {2021},
+ title =         {Caesar.jl, v0.16.3},
+ year =          {2024},
  doi=            {Solver DOI: 10.5281/zenodo.5146222}, 
  note =          {\url{https://github.com/JuliaRobotics/Caesar.jl}}
 }
diff --git a/Project.toml b/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.35.3"
+version = "0.35.4"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -64,18 +64,23 @@ IncrInfrInteractiveUtilsExt = "InteractiveUtils"
 IncrInfrInterpolationsExt = "Interpolations"
 
 [compat]
+AMD = "0.5"
 ApproxManifoldProducts = "0.7, 0.8"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
 DelimitedFiles = "1"
-DistributedFactorGraphs = "0.23"
+DifferentialEquations = "7"
+DistributedFactorGraphs = "0.23, 0.24"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"
 FiniteDiff = "2"
 FiniteDifferences = "0.12"
-FunctionalStateMachine = "0.2.9"
+Flux = "0.14"
+FunctionalStateMachine = "0.2.9, 0.3"
+Gadfly = "1"
+Interpolations = "0.15"
 JSON3 = "1"
 KernelDensityEstimate = "0.5.6"
 LinearAlgebra = "1.10"

diff --git a/README.md b/README.md
@@ -7,88 +7,92 @@
 | [![iif-ci-stb][iif-ci-stb-img]][iif-ci-stb-url] <br> [![version][iif-ver-img]][iif-rel-url] | [![iif-ci-dev-img]][iif-ci-dev-url] <br> [![iif-commits-url]][contributors-url] <br> [![issues-time]][issues-url] | [![doi-img]][doi-url] <br> [![iif-cov-img]][iif-cov-url] <br> [![issues-open]][issues-url] | [![cjl-slack-badge]][cjl-slack] <br> [![caesar-docs]][cjl-docs-url] <br> [![dfg-docs]][dfg-docs-url] |
 
 
-[iif-deps-img]: https://juliahub.com/docs/IncrementalInference/deps.svg
-[iif-deps-jlh]: https://juliahub.com/ui/Packages/IncrementalInference/NrVw2??page=2
-[doi-img]: https://zenodo.org/badge/DOI/10.5281/zenodo.7498643.svg
-[doi-url]: https://doi.org/10.5281/zenodo.7498643
-
-<!-- replicated in Caesar.jl README -->
-[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
-[iif-ci-stb-img]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml/badge.svg?branch=release%2Fv0.26
-[iif-ci-stb-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml
-[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
-[iif-cov-img]: https://codecov.io/github/JuliaRobotics/IncrementalInference.jl/coverage.svg?branch=master
-[iif-cov-url]: https://codecov.io/github/JuliaRobotics/IncrementalInference.jl?branch=master
+Optimization routines for incremental non-parametric and parametric solutions based on factor graphs and the Bayes (Junction) tree implemented in the [Julia language][jlorg-url].
 
-[iif-commits-url]: https://img.shields.io/github/commit-activity/y/JuliaRobotics/IncrementalInference.jl.svg?color=dark-green
-[contributors-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/graphs/contributors
-[issues-time]: https://isitmaintained.com/badge/resolution/JuliaRobotics/IncrementalInference.jl.svg
-[issues-open]: https://isitmaintained.com/badge/open/JuliaRobotics/IncrementalInference.jl.svg
-[issues-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/issues
-
-[caesar-docs]: https://img.shields.io/badge/CaesarDocs-latest-blue.svg
-[cjl-docs-url]: http://juliarobotics.github.io/Caesar.jl/latest/
-[dfg-docs]: https://img.shields.io/badge/DFGDocs-latest-blue.svg
-[dfg-docs-url]: https://juliarobotics.org/DistributedFactorGraphs.jl/latest/
-
-[cjl-slack-badge]: https://img.shields.io/badge/Caesarjl-Slack-green.svg?style=popout
-[cjl-slack]: https://join.slack.com/t/caesarjl/shared_invite/zt-ucs06bwg-y2tEbddwX1vR18MASnOLsw
-
-Optimization routines for incremental non-parametric and parametric solutions based on factor graphs and the Bayes (Junction) tree implemented in the [Julia language](http://www.julialang.org/) (and [JuliaPro](http://www.juliacomputing.com)).
-
-
-Introduction
-============
+# Introduction
 
 This package implements a few different non-Gaussian factor graph inference algorithms, primarily 
-- Multi-Modal iSAM (MM-iSAM) ([see references](http://www.juliarobotics.org/Caesar.jl/latest/refs/literature/)) which does hybrid non-parametric and parametric inference/state-estimation over large factor graphs.  
+- Multi-Modal iSAM (MM-iSAM) ([see references][cjl-docs-refs]) which does hybrid non-parametric and parametric inference/state-estimation over large factor graphs.  
 - Batch Parametric (akin to conventional "non-linear least squares"),
 - Max-mixtures parametric,
 - Other multiparametric and non-Gaussian algorithms are in the works and will be announced in due course.
 
 Fundamentally, inference is performed via the Bayes (junction) tree where Chapman-Kolmogorov transit integral solutions are based on marginal-joint belief estimation (a sum-product / belief-propagation approximation algorithm).  Many benefits such as clique recycling are also available.  See the common Caesar.jl documenation for more details.  [![caesar-docs]][cjl-docs-url]
 
-This package forms a cardinal piece of the [Caesar.jl](https://github.com/JuliaRobotics/Caesar.jl) robotics toolkit, including 3D visualization and database interaction, which can serve as a base station for a robotic platform. A standalone Robot Motion Estimate ([RoME.jl](https://github.com/JuliaRobotics/RoME.jl)) package extends the available variables, factors, and utilities for use in robotic navigation.  [![iif-deps-img]][iif-deps-jlh]  
+This package forms a cardinal piece of the [Caesar.jl][cjl-url] robotics toolkit, including 3D visualization and database interaction, which can serve as a base station for a robotic platform. A standalone Robot Motion Estimate ([RoME.jl][rjl-url]) package extends the available variables, factors, and utilities for use in robotic navigation.  [![iif-deps-img]][iif-deps-jlh]  
 
 Note, that IncrementalInference.jl **does not** have to be used with RoME.jl / Caesar.jl -- IncrementalInference.jl only implements the algebraic inference operations against mathematical abstractions such as Manifolds.jl. 
 
-Furthermore, please contact [email protected] for more formal support on this package, [NavAbility.io](https://www.navability.io). 
+Furthermore, please contact [email protected] for more formal support on this package, [NavAbility(TM) by WhereWhen.ai Technologies Inc.][wwai-url]. 
+
+# Examples
+
+See the common Caesar.jl documenation for more details [![caesar-docs]][cjl-docs-url].  Further examples can be found in the examples and test folders.
 
-Installation
-------------
+## Installation
 
 Install the package from inside Julia
 ```julia
-(v1.6) pkg> add IncrementalInference
+pkg> add IncrementalInference
 ```
 
+# Cite and Contributors
+
+We are grateful for many, many contributions within the Julia package ecosystem -- see the [`Project.toml`](https://github.com/JuliaRobotics/Caesar.jl/blob/master/Project.toml) files for a far reaching list of upstream packages and contributions.
+
+Consider citing our work using the common reference at [Caesar.jl Citation with IncrementalInference.jl DOI](https://github.com/JuliaRobotics/Caesar.jl#contributors)
+
+## Get Involved, and Code of Conduct
+
+This project adheres to the [JuliaRobotics code of conduct](https://github.com/JuliaRobotics/administration/blob/master/code_of_conduct.md), and we invite contributions or comments from the community.  Use the slack channel, Julia Discourse, or Github issues to get in touch.
+
+# References
+
+See [references of interest here][cjl-docs-refs]
+
+## Legacy
+
 Pre-install the following packages system wide packages[, and easily draw factor graph and Bayes tree]:
 ```bash
 sudo apt-get install hdf5-tools
 sudo apt-get install graphviz xdot # optional
 ```
 
-Examples
-========
 
-See the common Caesar.jl documenation for more details [![caesar-docs]][cjl-docs-url].  Further examples can be found in the examples and test folders.
+[iif-deps-img]: https://juliahub.com/docs/IncrementalInference/deps.svg
+[iif-deps-jlh]: https://juliahub.com/ui/Packages/IncrementalInference/NrVw2??page=2
+[doi-img]: https://zenodo.org/badge/DOI/10.5281/zenodo.5146221.svg
+[doi-url]: https://doi.org/10.5281/zenodo.5146221
 
-Cite and Contributors
-=====================
+[dfg-docs]: https://img.shields.io/badge/DFGDocs-latest-blue.svg
+[dfg-docs-url]: https://juliarobotics.org/DistributedFactorGraphs.jl/latest/
 
-We are grateful for many, many contributions within the Julia package ecosystem -- see the [`Project.toml`](https://github.com/JuliaRobotics/Caesar.jl/blob/master/Project.toml) files for a far reaching list of upstream packages and contributions.
+<!-- replicated in Caesar.jl README -->
+[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
+[iif-ci-stb-img]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml/badge.svg?branch=release%2Fv0.26
+[iif-ci-stb-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/actions/workflows/ci.yml
+[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
+[iif-cov-img]: https://codecov.io/github/JuliaRobotics/IncrementalInference.jl/coverage.svg?branch=master
+[iif-cov-url]: https://codecov.io/github/JuliaRobotics/IncrementalInference.jl?branch=master
 
-Consider citing our work using the common reference at [Caesar.jl Citation with IncrementalInference.jl DOI](https://github.com/JuliaRobotics/Caesar.jl#contributors)
+[iif-commits-url]: https://img.shields.io/github/commit-activity/y/JuliaRobotics/IncrementalInference.jl.svg?color=dark-green
+[contributors-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/graphs/contributors
+[issues-time]: https://isitmaintained.com/badge/resolution/JuliaRobotics/IncrementalInference.jl.svg
+[issues-open]: https://isitmaintained.com/badge/open/JuliaRobotics/IncrementalInference.jl.svg
+[issues-url]: https://github.com/JuliaRobotics/IncrementalInference.jl/issues
 
-Get Involved, and Code of Conduct
----------------------------------
+[rjl-url]: https://github.com/JuliaRobotics/RoME.jl
 
-This project adheres to the [JuliaRobotics code of conduct](https://github.com/JuliaRobotics/administration/blob/master/code_of_conduct.md), and we invite contributions or comments from the community.  Use the slack channel, Julia Discourse, or Github issues to get in touch.
+[cjl-url]: https://github.com/JuliaRobotics/Caesar.jl
+[caesar-docs]: https://img.shields.io/badge/CaesarDocs-latest-blue.svg
+[cjl-docs-url]: http://juliarobotics.github.io/Caesar.jl/latest/
+[cjl-docs-refs]: http://www.juliarobotics.org/Caesar.jl/latest/refs/literature/
 
-References
-==========
+[cjl-slack-badge]: https://img.shields.io/badge/Caesarjl-Slack-green.svg?style=popout
+[cjl-slack]: https://join.slack.com/t/caesarjl/shared_invite/zt-ucs06bwg-y2tEbddwX1vR18MASnOLsw
 
-See [references of interest here](http://www.juliarobotics.org/Caesar.jl/latest/refs/literature/)
+[jlorg-url]: http://www.julialang.org/
+[wwai-url]: https://www.wherewhen.ai
diff --git a/src/parametric/services/ParametricManopt.jl b/src/parametric/services/ParametricManopt.jl
@@ -7,14 +7,20 @@ using SparseArrays
 # using Zygote
 
 ##
-function getVarIntLabelMap(vartypeslist::OrderedDict{DataType, Vector{Symbol}})
+function getVarIntLabelMap(
+  vartypeslist::OrderedDict{DataType, Vector{Symbol}}
+)
   varlist_tuple = (values(vartypeslist)...,)
   varlabelsAP = ArrayPartition{Symbol, typeof(varlist_tuple)}(varlist_tuple)
   varIntLabel = OrderedDict(zip(varlabelsAP, collect(1:length(varlabelsAP))))
   return varIntLabel, varlabelsAP
 end
 
-function CalcFactorResidual(fg, fct::DFGFactor, varIntLabel)
+function CalcFactorResidual(
+  fg, 
+  fct::DFGFactor, 
+  varIntLabel
+)
   fac_func = getFactorType(fct)
   varOrder = getVariableOrder(fct)
 
@@ -45,7 +51,11 @@ end
   CalcFactorResidualAP
 Create an `ArrayPartition` of `CalcFactorResidual`s.
 """
-function CalcFactorResidualAP(fg::GraphsDFG, factorLabels::Vector{Symbol}, varIntLabel::OrderedDict{Symbol, Int64})
+function CalcFactorResidualAP(
+  fg::GraphsDFG, 
+  factorLabels::Vector{Symbol}, 
+  varIntLabel::OrderedDict{Symbol, Int64}
+)
   factypes, typedict, alltypes = getFactorTypesCount(getFactor.(fg, factorLabels))
 
   # skip non-numeric prior (MetaPrior)
@@ -300,6 +310,7 @@ function solve_RLM(
   faclabels = lsf(fg);
   is_sparse = true,
   finiteDiffCovariance = false,
+  solveKey::Symbol = :parametric,
   kwargs...
 )
 
@@ -312,7 +323,7 @@ function solve_RLM(
 
   #Can use varIntLabel (because its an OrderedDict), but varLabelsAP makes the ArrayPartition.
   p0 = map(varlabelsAP) do label
-    getVal(fg, label, solveKey = :parametric)[1]
+    getVal(fg, label; solveKey)[1]
   end
 
   # create an ArrayPartition{CalcFactorResidual} for faclabels
@@ -389,6 +400,7 @@ function solve_RLM_conditional(
   separators::Vector{Symbol} = setdiff(ls(fg), frontals);
   is_sparse=false,
   finiteDiffCovariance=true,
+  solveKey::Symbol = :parametric,
   kwargs...
 )
   is_sparse && error("Sparse solve_RLM_conditional not supported yet")
@@ -420,7 +432,7 @@ function solve_RLM_conditional(
   all_varlabelsAP = ArrayPartition((frontal_varlabelsAP.x..., separator_varlabelsAP.x...))
 
   all_points = map(all_varlabelsAP) do label
-    getVal(fg, label, solveKey = :parametric)[1]
+    getVal(fg, label; solveKey)[1]
   end
 
   p0 = ArrayPartition(all_points.x[1:length(frontal_varlabelsAP.x)])
@@ -488,10 +500,11 @@ function autoinitParametric!(
   reinit = false,
   kwargs...
 )
-  @showprogress for vIdx in varorderIds
+  init_labels = @showprogress map(varorderIds) do vIdx
     autoinitParametric!(fg, vIdx; reinit, kwargs...)
   end
-  return nothing
+  filter!(!isnothing, init_labels)
+  return init_labels
 end
 
 function autoinitParametric!(dfg::AbstractDFG, initme::Symbol; kwargs...)
@@ -520,6 +533,11 @@ function autoinitParametric!(
     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
+    end
 
     vnd::VariableNodeData = getSolverData(xi, solveKey)
 
@@ -537,25 +555,25 @@ function autoinitParametric!(
         )
       )
     end
-    M, vartypeslist, lm_r, Σ = solve_RLM_conditional(dfg, [initme], initfrom; kwargs...)
+    M, vartypeslist, lm_r, Σ = solve_RLM_conditional(dfg, [initme], initfrom; solveKey, kwargs...)
 
     val = lm_r[1]
     vnd.val[1] = val
 
-    !isnothing(Σ) && vnd.bw .= Σ
+    !isnothing(Σ) && (vnd.bw .= Σ)
 
     # updateSolverDataParametric!(vnd, val, Σ)
 
     vnd.initialized = true
     #fill in ppe as mean
     Xc::Vector{Float64} = collect(getCoordinates(getVariableType(xi), val))
-    ppe = MeanMaxPPE(:parametric, Xc, Xc, Xc)
-    getPPEDict(xi)[:parametric] = ppe
+    ppe = MeanMaxPPE(solveKey, Xc, Xc, Xc)
+    getPPEDict(xi)[solveKey] = ppe
 
-    result = vartypeslist, lm_r
+    result = true
 
   else
-    result = nothing
+    result = false
   end
 
   return result#isInitialized(xi, solveKey)

diff --git a/test/testBasicParametric.jl b/test/testBasicParametric.jl
@@ -53,6 +53,7 @@ v2 = vardict[:x2]
 @test isapprox(v2.cov, [0.125;;], atol=1e-3)
 initVariable!(fg, :x2, Normal(v2.val[1], sqrt(v2.cov[1])), :parametric)
 
+addFactor!(fg, [:x0], Prior(Normal(0.1,1.1)))
 IIF.solveGraphParametric!(fg; is_sparse=false)
 
 end
@@ -75,15 +76,13 @@ end
 
 fg = generateGraph_LineStep(2, graphinit=true, vardims=1, poseEvery=1, landmarkEvery=0, posePriorsAt=Int[0], sightDistance=3, solverParams=SolverParams(algorithms=[:default, :parametric]))
 
-r = IIF.autoinitParametric!(fg, :x0)
-@test_broken IIF.Optim.converged(r)
+@test IIF.autoinitParametric!(fg, :x0)
 
 v0 = getVariable(fg,:x0)
 @test length(v0.solverDataDict[:parametric].val[1]) === 1
 @test isapprox(v0.solverDataDict[:parametric].val[1][1], 0.0, atol = 1e-4)
 
-r = IIF.autoinitParametric!(fg, :x1)
-@test_broken IIF.Optim.converged(r)
+@test IIF.autoinitParametric!(fg, :x1)
 
 v0 = getVariable(fg,:x1)
 @test length(v0.solverDataDict[:parametric].val[1]) === 1

diff --git a/test/testMixtureLinearConditional.jl b/test/testMixtureLinearConditional.jl
@@ -91,8 +91,8 @@ f1 = addFactor!(fg, [:x0;:x1], mr)
 
 ##
 
-pf0 = DFG.packFactor(fg, f0)
-pf1 = DFG.packFactor(fg, f1)
+pf0 = DFG.packFactor(f0)
+pf1 = DFG.packFactor(f1)
 
 # now test unpacking
 fg_ = initfg();

diff --git a/test/testPackingMixtures.jl b/test/testPackingMixtures.jl
@@ -23,7 +23,7 @@ addFactor!(fg, [:x0, :x1], mmo)
 
 ##
 
-pf = packFactor(fg, getFactor(fg, :x0x1f1))
+pf = packFactor(getFactor(fg, :x0x1f1))
 
 ##
 

diff --git a/test/testSpecialEuclidean2Mani.jl b/test/testSpecialEuclidean2Mani.jl
@@ -436,10 +436,12 @@ solveGraph!(fg);
 
 ## check saveDFG (check consistency of packing converters above)
 
-
-saveDFG(joinpath(tempdir(),"passthru"), fg)
-fg_ = loadDFG(joinpath(tempdir(),"passthru.tar.gz"))
-Base.rm(joinpath(tempdir(),"passthru.tar.gz"))
+@error "Whats going on in PackedManifoldPrior, skipping tests"
+@test_broken begin
+    saveDFG(joinpath(tempdir(),"passthru"), fg)
+    fg_ = loadDFG(joinpath(tempdir(),"passthru.tar.gz"))
+    Base.rm(joinpath(tempdir(),"passthru.tar.gz"))
+end
 
 # @error "#FIXME test propagateBelief w HeatmapSampler ... broken on ci but not local"
 # return true