Restore ForwardDiff support in parametric and use precision matrices (#1915)

* Update belief topology trait

* ForwardDiff support in solve_RLM and use precision matrices

* tmp: don't test upstream-dev AMP develop

---------

Co-authored-by: Johannes Terblanche <Affie@users.noreply.github.com>

Author: Affie

.github/workflows/ci.yml

@@ -84,7 +84,7 @@ jobs:
         run: |
           using Pkg;
           Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"))
-          Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));
+          # Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));
           Pkg.develop([PackageSpec(path="IncrementalInference"), PackageSpec(path="IncrementalInferenceTypes")]);
       - name: Run tests
         continue-on-error: ${{ matrix.group == 'tmp_debug_group' }}

.gitignore

@@ -9,3 +9,5 @@ Manifest.toml
 *.cov
 lcov.info
 benchmark/results.md
+benchmark/#*
+

IncrementalInference/Project.toml

@@ -20,6 +20,7 @@ DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
 FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41"
 FiniteDifferences = "26cc04aa-876d-5657-8c51-4c34ba976000"
+ForwardDiff = "f6369f11-7733-5829-9624-2563aa707210"
 FunctionalStateMachine = "3e9e306e-7e3c-11e9-12d2-8f8f67a2f951"
 IncrementalInferenceTypes = "9808408f-4dbc-47e4-913c-6068b950e289"
 JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1"
@@ -83,6 +84,7 @@ FileIO = "1"
 FiniteDiff = "2"
 FiniteDifferences = "0.12"
 Flux = "0.14, 0.15, 0.16"
+ForwardDiff = "1.3.3"
 FunctionalStateMachine = "0.2.9, 0.3"
 Gadfly = "1"
 IncrementalInferenceTypes = "0.1.0"

IncrementalInference/src/entities/BeliefTypes.jl

@@ -78,10 +78,10 @@ function TreeBelief(
 end
 
 function TreeBelief(vnd::State, solvDim::Real = 0)
-  TreeBelief(DFG.getDensityKind(vnd), vnd, solvDim)
+  TreeBelief(DFG.getTopologyKind(vnd), vnd, solvDim)
 end
 
-function TreeBelief(::DFG.GaussianDensityKind, vnd::State, solvDim::Real = 0)
+function TreeBelief(::DFG.RootsOnlyTopology, vnd::State, solvDim::Real = 0)
   return TreeBelief(
     DFG.refMeans(vnd),
     DFG.refCovariances(vnd)[1],
@@ -92,7 +92,7 @@ function TreeBelief(::DFG.GaussianDensityKind, vnd::State, solvDim::Real = 0)
   )
 end
 
-function TreeBelief(::DFG.NonparametricDensityKind, vnd::State, solvDim::Real = 0)
+function TreeBelief(::DFG.LeavesOnlyTopology, vnd::State, solvDim::Real = 0)
   return TreeBelief(
     DFG.refPoints(vnd),
     DFG.refBandwidth(vnd),

IncrementalInference/src/manifolds/services/ManifoldsExtentions.jl

@@ -55,6 +55,18 @@ function Manifolds.get_vector!(M::NPowerManifold, Y, p, c, B::AbstractBasis)
   return Y
 end
 
+# Allocating get_vector that infers element type from coordinates (Dual-compatible)
+function ManifoldsBase.get_vector(M::NPowerManifold, p, c, B::AbstractBasis)
+  dim = manifold_dimension(M.manifold)
+  rep_size = Manifolds.representation_size(M.manifold)
+  v_iter = Ref(0)
+  return [begin
+    coords_i = SVector{dim}(view(c, v_iter[]+1:v_iter[]+dim))
+    v_iter[] += dim
+    get_vector(M.manifold, Manifolds._read(M, rep_size, p, i), coords_i, B)
+  end for i in Manifolds.get_iterator(M)]
+end
+
 function Manifolds.exp!(M::NPowerManifold, q, p, X)
   rep_size = Manifolds.representation_size(M.manifold)
   for i in Manifolds.get_iterator(M)
@@ -67,6 +79,16 @@ function Manifolds.exp!(M::NPowerManifold, q, p, X)
   return q
 end
 
+# Allocating exp that infers element type from tangent vector (Dual-compatible)
+function ManifoldsBase.exp(M::NPowerManifold, p, X)
+  rep_size = Manifolds.representation_size(M.manifold)
+  return [exp(
+    M.manifold,
+    Manifolds._read(M, rep_size, p, i),
+    Manifolds._read(M, rep_size, X, i),
+  ) for i in Manifolds.get_iterator(M)]
+end
+
 function LieGroups.compose!(M::NPowerManifold, x, p, q)
   rep_size = representation_size(M.manifold)
   for i in Manifolds.get_iterator(M)