Merge pull request #217 from JuliaRobotics/master

v0.6.1-rc1

.github/workflows/ci.yml

@@ -16,7 +16,7 @@ jobs:
       matrix:
         version:
           - '1.6'
-          - '1.7'
+          - '1.8'
           - 'nightly'
         os:
           - ubuntu-latest

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.6.0"
+version = "0.6.1"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
@@ -24,6 +24,7 @@ Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
 RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd"
 Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
 Requires = "ae029012-a4dd-5104-9daa-d747884805df"
+Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 Sockets = "6462fe0b-24de-5631-8697-dd941f90decc"
 SparseArrays = "2f01184e-e22b-5df5-ae63-d93ebab69eaf"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
@@ -44,6 +45,7 @@ Optim = "1"
 RecursiveArrayTools = "2"
 Reexport = "0.2, 1.0"
 Requires = "0.5, 1"
+Rotations = "1"
 StaticArrays = "0.15, 1"
 TensorCast = "0.2, 0.3, 0.4"
 TransformUtils = "0.2.10"

src/ApproxManifoldProducts.jl

@@ -2,15 +2,16 @@ module ApproxManifoldProducts
 
 using Reexport
 @reexport using KernelDensityEstimate
-@reexport using TransformUtils  # likely to be deprecated
 using Random
 
+import TransformUtils as TUs
+import Rotations as _Rot
+
 import ManifoldsBase
 import ManifoldsBase: AbstractManifold
 using RecursiveArrayTools: ArrayPartition
 export ArrayPartition
 
-const MB = ManifoldsBase
 using Manifolds
 
 using DocStringExtensions
@@ -25,45 +26,23 @@ using StaticArrays
 using Logging
 using Statistics
 
+import Random: rand
+
 import Base: *, isapprox, convert
 import LinearAlgebra: rotate!
-import Statistics: mean
+import Statistics: mean, std, cov, var
 import KernelDensityEstimate: getPoints, getBW
-import TransformUtils: rotate!
 
+const MB = ManifoldsBase
+const CTs = CoordinateTransformations
 const AMP = ApproxManifoldProducts
 const KDE = KernelDensityEstimate
-const TUs = TransformUtils
-const CTs = CoordinateTransformations
 
 # TODO temporary for initial version of on-manifold products
 KDE.setForceEvalDirect!(true)
 
-export  
-  # new local features
-  AMP,
-  MKD,
-  AbstractManifold,
-  ManifoldKernelDensity,
-  get2DLambda,
-  get2DMu,
-  get2DMuMin,
-  resid2DLinear,
-  solveresid2DLinear!,
-  solveresid2DLinear,
-  *,
-  isapprox,
-
-  # APi and util functions
-  buildHybridManifoldCallbacks,
-  getKDEManifoldBandwidths,
-  manifoldProduct,
-  manikde!,
-  calcCovarianceBasic,
-  isPartial,
-  mean,
-  calcProductGaussians
-
+# the exported API
+include("ExportAPI.jl")
 
 # internal features not exported
 include("_BiMaps.jl")

src/Deprecated.jl

@@ -1,4 +1,13 @@
 
+## ======================================================================================================
+## Remove below before v0.8
+## ======================================================================================================
+
+
+@deprecate R(th::Real) _Rot.RotMatrix2(th).mat # = [[cos(th);-sin(th)]';[sin(th);cos(th)]'];
+@deprecate R(;x::Real=0.0,y::Real=0.0,z::Real=0.0) (M=SpecialOrthogonal(3);exp(M,identity_element(M),hat(M,Identity(M),[x,y,z]))) # convert(SO3, so3([x,y,z]))
+
+
 ## ======================================================================================================
 ## Remove below before v0.7
 ## ======================================================================================================

src/ExportAPI.jl

@@ -0,0 +1,32 @@
+
+export  
+  # new local features
+  AMP,
+  MKD,
+  AbstractManifold,
+  ManifoldKernelDensity,
+  get2DLambda,
+  get2DMu,
+  get2DMuMin,
+  resid2DLinear,
+  solveresid2DLinear!,
+  solveresid2DLinear,
+  *,
+  isapprox,
+
+  # APi and util functions
+  buildHybridManifoldCallbacks,
+  getKDEManifoldBandwidths,
+  manifoldProduct,
+  manikde!,
+  calcCovarianceBasic,
+  isPartial,
+  calcProductGaussians
+
+export getPoints, getBW, Ndim, Npts
+export getKDERange, getKDEMax, getKDEMean, getKDEfit
+export sample, rand, resample, kld, minkld
+export calcMean
+export mean, cov, std, var
+export getInfoPerCoord, getBandwidth
+export antimarginal

src/plotting/CircularPlotting.jl

@@ -29,6 +29,9 @@ function plotCircBeliefs( arr::V;
     beliefs[j] = logpdf ? (x)->log(ar(x)+1.0) : (x)->ar(x) #
   end
 
+  M = SpecialOrthogonal(2)
+  e0 = Identity(M)
+
   PL = []
   # TODO loosing some modes in plotting???
   for j in 1:length(beliefs)
@@ -42,7 +45,7 @@ function plotCircBeliefs( arr::V;
       # second term directional statistics for plotting (not evaluation)
       obj[i] += (j != 1 ? beliefs[j](t-2pi) : 0.0 )
       obj[i] += (j != 1 ? beliefs[j](t+2pi) : 0.0 )
-      xy = TransformUtils.R(rVo[3])*[cos(t)*obj[i]; sin(t)*obj[i]]
+      xy = exp(M,e0,hat(M,e0,rVo[3]))*[cos(t)*obj[i]; sin(t)*obj[i]]
       X[i] = xy[1] + rVo[1]
       Y[i] = xy[2] + rVo[2]
     end

src/services/ManifoldKernelDensity.jl

@@ -1,14 +1,4 @@
 
-import Random: rand
-
-export getPoints, getBW, Ndim, Npts
-export getKDERange, getKDEMax, getKDEMean, getKDEfit
-export sample, rand, resample, kld, minkld
-export calcMean
-export getInfoPerCoord, getBandwidth
-export antimarginal
-
-
 ## ==========================================================================================
 ## helper functions to contruct MKD objects
 ## ==========================================================================================
@@ -45,15 +35,16 @@ ManifoldKernelDensity(mani::M,
 
 function ManifoldKernelDensity( M::MB.AbstractManifold,
                                 vecP::AbstractVector{P},
-                                u0=vecP[1];
+                                u0=vecP[1],
+                                ϵ = identity_element(M, vecP[1]);
                                 partial::L=nothing,
                                 infoPerCoord::AbstractVector{<:Real}=ones(getNumberCoords(M, u0)),
                                 dims::Int=manifold_dimension(M),
                                 bw::Union{<:AbstractVector{<:Real},Nothing}=nothing  ) where {P,L}
   #
   # FIXME obsolete
   arr = Matrix{Float64}(undef, dims, length(vecP))
-  ϵ = identity_element(M, vecP[1])
+
 
   for j in 1:length(vecP)
     arr[:,j] = vee(M, ϵ, log(M, ϵ, vecP[j]))
@@ -86,14 +77,18 @@ manikde!( M::MB.AbstractManifold,
 ## a few utilities
 ## ==========================================================================================
 
-function Statistics.mean(mkd::ManifoldKernelDensity, aspartial::Bool=true)
-  M = if aspartial && isPartial(mkd)
+# TODO this should be a public method relating to getManifold
+function _getManifoldFullOrPart(mkd::ManifoldKernelDensity, aspartial::Bool=true)
+  if aspartial && isPartial(mkd)
     getManifoldPartial(mkd.manifold, mkd._partial)
   else
     mkd.manifold
   end
+end
 
-  mean(mkd.manifold, getPoints(mkd, aspartial))
+function Statistics.mean(mkd::ManifoldKernelDensity, aspartial::Bool=true)
+  M = _getManifoldFullOrPart(mkd, aspartial)
+  mean(M, getPoints(mkd, aspartial))
 end
 
 """
@@ -103,6 +98,18 @@ Alias for overloaded `Statistics.mean`.
 """
 calcMean(mkd::ManifoldKernelDensity, aspartial::Bool=true) = mean(mkd, aspartial)
 
+function Statistics.std(mkd::ManifoldKernelDensity, aspartial::Bool=true; kwargs...)
+  std(_getManifoldFullOrPart(mkd,aspartial), getPoints(mkd, aspartial); kwargs...)
+end
+function Statistics.var(mkd::ManifoldKernelDensity, aspartial::Bool=true; kwargs...)
+  var(_getManifoldFullOrPart(mkd,aspartial), getPoints(mkd, aspartial); kwargs...)
+end
+
+function Statistics.cov(mkd::ManifoldKernelDensity, aspartial::Bool=true; basis::Manifolds.AbstractBasis = Manifolds.DefaultOrthogonalBasis(), kwargs...)
+  return cov(_getManifoldFullOrPart(mkd,aspartial), getPoints(mkd, aspartial); basis, kwargs... )
+end
+
+
 
 _getFieldPartials(mkd::ManifoldKernelDensity{M,B,Nothing}, field::Function, aspartial::Bool=true) where {M,B} = field(mkd)