fix solveFactorParametric as coordinates only

Author: dehann

src/TetherUtils.jl

@@ -111,6 +111,7 @@ Notes
 - Not used during tree inference.
 - Expected uses are for user analysis of factors and estimates.
 - real-time dead reckoning chain prediction.
+- Returns mean value as coordinates
 
 DevNotes
 - # TODO consolidate with similar `approxConv`

src/services/ApproxConv.jl

@@ -276,9 +276,13 @@ function solveFactorParameteric(dfg::AbstractDFG,
 
   # get the measurement point
   fctTyp = getFactorType(fct)
+  # this is definitely in coordinates, see JuliaRobotics/RoME.jl#465
   mea, _ = getMeasurementParametric(fctTyp)
-  # should this be coords, tangent, or point
-  measT = (mea,)
+  # must change measT to be a tangent vector
+  M = getManifold(fctTyp)
+  e0 = identity_element(M)
+  mea_ = hat(M, e0, mea)
+  measT = (mea_,)
 
   # get variable points
   function _getParametric(vari::DFGVariable, key=:default)