Merge pull request #1349 from JuliaRobotics/21Q3/fix/getmeasparam_ascoord

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)

test/testDeadReckoningTether.jl

@@ -21,7 +21,7 @@ MutableLinearRelative(nm::MvNormal) = MutableLinearRelative{length(nm.μ), typeo
 MutableLinearRelative(nm::ManifoldKernelDensity) = MutableLinearRelative{Ndim(nm), typeof(nm)}(nm)
 
 getDimension(::Type{MutableLinearRelative{N,<:SamplableBelief}}) where {N} = N
-# getManifolds(::Type{MutableLinearRelative{N,<:SamplableBelief}}) where {N} = tuple([:Euclid for i in 1:N]...)
+getManifold(::MutableLinearRelative{N}) where N = TranslationGroup(N)
 
 
 function IIF.getSample(cf::CalcFactor{<:MutableLinearRelative}, N::Int=1)