ForwardDiff support in solve_RLM and use precision matrices

Author: Affie

.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/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)