Merge pull request #172 from JuliaRobotics/master

v0.4.19-rc1

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.18"
+version = "0.4.19"
 
 [deps]
 Base64 = "2a0f44e3-6c83-55bd-87e4-b1978d98bd5f"
@@ -53,7 +53,8 @@ julia = "1.4"
 DataFrames = "a93c6f00-e57d-5684-b7b6-d8193f3e46c0"
 FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549"
 JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
+Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["DataFrames", "FileIO", "JLD2", "Test"]
+test = ["DataFrames", "FileIO", "JLD2", "Rotations", "Test"]

src/Interface.jl

@@ -17,23 +17,28 @@ Manifolds.identity_element(::Circle, val::AbstractVector{T}) where {T <: Real} =
 
 Helper function to convert coordinates to a desired on-manifold point.
 
+DevNotes
+- FIXME need much better consolidation or even removal of this function entirely.
+  - This function makes too strong an assumption of groups
+
 Notes
 - `u0` is used to identify the data type for a point
 - Pass in a different `exp` if needed.
 """
-makePointFromCoords(M::MB.AbstractManifold,
+makePointFromCoords(M::MB.AbstractManifold,  # Manifolds.AbstractGroupManifold
                     coords::AbstractVector{<:Real},
                     u0=zeros(manifold_dimension(M)),
                     ϵ=identity_element(M,u0),
                     retraction_method::AbstractRetractionMethod=ExponentialRetraction()  ) = retract(M, ϵ, hat(M, ϵ, coords), retraction_method)
 #
 
 # should perhaps just be dispatched for <:AbstractGroupManifold
+# only works for AbstractGroupManifold (have an identity)
 function makeCoordsFromPoint( M::MB.AbstractManifold,
-                              pt::P ) where P
+                              pt::P,
+                              ϵ = identity_element(M, pt) ) where P
   #
-  # only works for manifold which have an identity (eg groups)
-  ϵ = identity_element(M, pt)
+
   vee(M, ϵ, log(M, ϵ, pt))
 end
 

src/KernelHilbertEmbeddings.jl

@@ -12,6 +12,11 @@ Normal kernel used for Hilbert space embeddings.
 """
 ker(M::MB.AbstractManifold, p, q, sigma::Real=0.001) = exp( -sigma*(distance(M, p, q)^2) )
 
+# overwrite non-symmetric with alternate implementations 
+# ker(M::MB.AbstractManifold, p, q, sigma::Real=0.001) = exp( -sigma*(distance(M, p, q)^2) )
+
+
+
 """
     $SIGNATURES
 
@@ -100,6 +105,6 @@ function mmd(a::ManifoldKernelDensity{M}, b::ManifoldKernelDensity{M}; bw::Vecto
 end
 
 
-function isapprox(a::ManifoldKernelDensity, b::ManifoldKernelDensity; atol::Real=0.1)
+function isapprox(a::ManifoldKernelDensity, b::ManifoldKernelDensity; mmd_tol::Real=1e-1, atol::Real=mmd_tol)
   mmd(a,b) < atol
 end

src/services/ManifoldKernelDensity.jl

@@ -196,7 +196,7 @@ end
 
 
 # TODO check that partials / marginals are sampled correctly
-function sample(x::ManifoldKernelDensity{M,B,L,P}, N::Int) where {M,B,L,P}
+function sample(x::ManifoldKernelDensity{M,B,L,P}, N::Integer=1) where {M,B,L,P}
   # get legacy matrix of coordinates and selected labels
   coords, lbls = sample(x.belief, N)
 

test/runtests.jl

@@ -6,12 +6,14 @@ using Test
 include("basics.jl")
 include("ex_1D.jl")
 include("ex_2D_rot.jl")
+include("testManifoldConventions.jl")
 include("testManifoldPartial.jl")
 include("testManiProductBigSmall.jl")
 include("testBasicManiProduct.jl")
 include("testMarginalProducts.jl")
 include("testMMD.jl")
 include("testPartialProductSE2.jl")
 include("basic_se3.jl")
+include("testSymmetry.jl")
 
 #

test/testManifoldConventions.jl

@@ -0,0 +1,67 @@
+# test manifold conventions
+
+
+using Test
+using Manifolds
+
+import Rotations as _Rot
+
+##
+@testset "test Manifolds.jl conventions" begin
+##
+
+M = SpecialEuclidean(2)
+e0 = identity_element(M)
+
+# body to body next
+bTb_ = ProductRepr([10.0;0], _Rot.RotMatrix(pi/2))
+
+# drive in a clockwise square from the origin
+wTb0 = ProductRepr([100.0;0], _Rot.RotMatrix(0.0))
+wTb1 = Manifolds.compose(M, wTb0, bTb_)    # right
+wTb2 = Manifolds.compose(M, wTb1, bTb_)    # top right
+wTb3 = Manifolds.compose(M, wTb2, bTb_)    # top
+wTb4 = Manifolds.compose(M, wTb3, bTb_)    # origin
+
+wCb0 = vee(M, e0, log(M, e0, wTb0))
+wCb1 = vee(M, e0, log(M, e0, wTb1))
+wCb2 = vee(M, e0, log(M, e0, wTb2))
+wCb3 = vee(M, e0, log(M, e0, wTb3))
+wCb4 = vee(M, e0, log(M, e0, wTb4))
+
+##
+
+# check the favorable result
+@test isapprox( [100,0.,0],    wCb0 ; atol=1e-6 )
+@test isapprox( [110,0.,pi/2], wCb1 ; atol=1e-6 )
+@test isapprox( [110.,10,pi],  wCb2 ; atol=1e-6 ) || isapprox( [110,10,-pi], wCb2 ; atol=1e-6 )
+@test isapprox( [100.,10,-pi/2], wCb3 ; atol=1e-6 )
+@test isapprox( [100,0,0.], wCb4 ; atol=1e-6 )
+
+
+## check that the inverse breaks
+
+
+# Use opposite convention from above to show it is wrong
+wTb0 = ProductRepr([100.0;0], _Rot.RotMatrix(0.0))
+wTb1 = Manifolds.compose(M, bTb_, wTb0)    # right
+wTb2 = Manifolds.compose(M, bTb_, wTb1)    # top right
+wTb3 = Manifolds.compose(M, bTb_, wTb2)    # top
+wTb4 = Manifolds.compose(M, bTb_, wTb3)    # origin
+
+wCb0 = vee(M, e0, log(M, e0, wTb0))
+wCb1 = vee(M, e0, log(M, e0, wTb1))
+wCb2 = vee(M, e0, log(M, e0, wTb2))
+wCb3 = vee(M, e0, log(M, e0, wTb3))
+wCb4 = vee(M, e0, log(M, e0, wTb4))
+
+# check the negative result
+@test  isapprox( [100,0.,0],      wCb0 ; atol=1e-6 )
+@test !isapprox( [110,0.,pi/2],   wCb1 ; atol=1e-6 )
+@test !(isapprox( [110.,10,pi],    wCb2 ; atol=1e-6 ) || isapprox( [110,10,-pi], wCb2 ; atol=1e-6 ))
+@test !isapprox( [100.,10,-pi/2], wCb3 ; atol=1e-6 )
+@test  isapprox( [100,0,0.],      wCb4 ; atol=1e-6 )
+
+
+##
+end

test/testSymmetry.jl

@@ -0,0 +1,34 @@
+# test for symmetry on distances
+
+using Manifolds
+using Test
+
+import Rotations as _Rot
+
+##
+@testset "test symmetry of Manifolds.distance" begin
+##
+
+M = TranslationGroup(2)
+a,b = randn(2), randn(2)
+@test isapprox( distance(M, a, b), distance(M, b, a), atol = 1e-5)
+
+
+M = SpecialOrthogonal(2)
+a,b = _Rot.RotMatrix(randn()), _Rot.RotMatrix(randn())
+@test isapprox( distance(M, a, b), distance(M, b, a), atol = 1e-5)
+
+
+M = SpecialEuclidean(2)
+a = ProductRepr(randn(2),_Rot.RotMatrix(randn()))
+b = ProductRepr(randn(2),_Rot.RotMatrix(randn()))
+@test isapprox( distance(M, a, b), distance(M, b, a), atol = 1e-5)
+
+
+M = SpecialOrthogonal(3)
+a = _Rot.RotZ(randn())*_Rot.RotY(randn())*_Rot.RotX(randn())
+b = _Rot.RotZ(randn())*_Rot.RotY(randn())*_Rot.RotX(randn())
+@test isapprox( distance(M, a, b), distance(M, b, a), atol = 1e-5)
+
+##
+end