Merge pull request #110 from JuliaRobotics/21Q3/upstream/calccovbasic

add calcVariableCovarianceBasic, upstream from IIF

Project.toml

@@ -1,7 +1,7 @@
 name = "ApproxManifoldProducts"
 uuid = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
 keywords = ["MM-iSAMv2", "SLAM", "inference", "sum-product", "belief-propagation", "nonparametric", "manifolds", "functional"]
-version = "0.4.5"
+version = "0.4.6"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"

src/ApproxManifoldProducts.jl

@@ -56,7 +56,8 @@ export
   buildHybridManifoldCallbacks,
   getKDEManifoldBandwidths,
   manifoldProduct,
-  manikde!
+  manikde!,
+  calcVariableCovarianceBasic
 
 
 # internal features not exported

src/CommonUtils.jl

@@ -60,6 +60,20 @@ function updateProductSample( dest::BallTreeDensity,
   manikde!(pts, manifolds)
 end
 
+# Returns the covariance (square), not deviation
+function calcVariableCovarianceBasic(M::AbstractManifold, ptsArr::Vector{P}) where P
+  #TODO double check the maths,. it looks like its working at least for groups
+  μ = mean(M, ptsArr)
+  Xcs = vee.(Ref(M), Ref(μ), log.(Ref(M), Ref(μ), ptsArr))
+  Σ = mean(Xcs .* transpose.(Xcs))
+  @debug "calcVariableCovarianceBasic" μ
+  @debug "calcVariableCovarianceBasic" Σ
+  # TODO don't know what to do here so keeping as before, #FIXME it will break
+  # a change between this and previous is that a full covariance matrix is returned
+  msst = Σ
+  msst_ = 0 < sum(1e-10 .< msst) ? maximum(msst) : 1.0
+  return msst_
+end
 
 
 # """