stash on par_tree

Author: Affie

IncrementalInference/src/Factors/GenericFunctions.jl

@@ -123,6 +123,15 @@ function Ad(::typeof(SpecialEuclidean(2)), p)
   )
 end
 
+function Ad(::Motion(2), p)
+  t = p.x[1]
+  R = p.x[2]
+  vcat(
+      hcat(R, -SA[0 -1; 1 0]*t),
+      SA[0 0 1]
+  )
+end
+
 struct AdFactor{F <: AbstractManifoldMinimize} <: AbstractManifoldMinimize
   factor::F
 end

IncrementalInference/src/services/FactorGradients.jl

@@ -31,7 +31,7 @@ function factorJacobian(
   M_codom = Euclidean(manifold_dimension(getManifold(fac)))
   # Jx(M, p) = ManifoldDiff.jacobian(M, M_codom, calcfac, p, backend)
 
-  return ManifoldDiff.jacobian(M_dom, M_codom, costf, p0, backend)
+  return ManifoldDiff.jacobian(M_dom, M_codom, costf, p0, backend), costf(p0)
 end