AMP v0.3, towards Manifolds.jl, add SE2E2

.github/workflows/ci.yml

@@ -14,8 +14,8 @@ jobs:
       fail-fast: false
       matrix:
         version:
-          - '1.5'
-          - '^1.6.0-0'
+          - '1.6'
+          - 'nightly'
         os:
           - ubuntu-latest
         arch:
@@ -56,7 +56,7 @@ jobs:
 
       - uses: julia-actions/setup-julia@v1
         with:
-          version: 1.5
+          version: 1.6
           arch: x64
 
       - uses: actions/cache@v1

Project.toml

@@ -18,6 +18,7 @@ IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
 JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
+Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
 Optim = "429524aa-4258-5aef-a3af-852621145aeb"
 ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca"
@@ -30,7 +31,7 @@ TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 
 [compat]
-ApproxManifoldProducts = "0.1.4, 0.2"
+ApproxManifoldProducts = "0.3.1"
 CoordinateTransformations = "0.5, 0.6, 0.7"
 DistributedFactorGraphs = "0.12"
 Distributions = "0.21, 0.22, 0.23, 0.24"
@@ -39,13 +40,14 @@ FileIO = "1.0.2, 1.1, 1.2"
 IncrementalInference = "0.21.2, 0.22"
 JLD2 = "0.2, 0.3, 0.4"
 KernelDensityEstimate = "0.5.1, 0.6"
+Manifolds = "0.4.19"
 ManifoldsBase = "0.10"
 Optim = "0.22, 1.0"
 ProgressMeter = "1"
 Reexport = "0.2, 1.0"
 Requires = "1.0"
 Rotations = "0.12.1, 0.13, 1.0"
-TensorCast = "0.2, 0.3"
+TensorCast = "0.2, 0.3, 0.4"
 TransformUtils = "0.2.7"
 julia = "1.4"
 

src/Deprecated.jl

@@ -1,31 +1,42 @@
 
+
+
 ##==============================================================================
-## Remve as part of Manifolds.jl consolidation, #244
+## Remove as part of Manifolds.jl consolidation, #244
 ##==============================================================================
 
 
-## New Manifold types.  Integration phase towards RoME #244 and AMP #41
+## New Manifold types.  Integration phase towards RoME #244 and AMP #32 / #41
 
 # FIXME, much consolidation required here, see RoME #244
-
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Point2}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Point3}) = AMP.Euclid3
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:DynPoint2}) = AMP.Euclid4
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose2}) = AMP.SE2_Manifold
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose3}) = AMP.SE3_Manifold
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:DynPose2}) = AMP.SE2E2_Manifold
-
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Point2Point2}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose2Point2}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose2Point2Bearing}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Point2Point2Range}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose2Point2Range}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose2Point2BearingRange}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose2Pose2}) = AMP.SE2_Manifold
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:Pose3Pose3}) = AMP.SE3_Manifold
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:DynPoint2DynPoint2}) = AMP.Euclid4
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:DynPose2DynPose2}) = AMP.SE2E2_Manifold
-Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{<:VelPose2VelPose2}) = AMP.SE2E2_Manifold
+import IncrementalInference: getManifolds
+
+# TODO consolidate Manifolds typing and objects
+getManifolds(::InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Circular)
+getManifolds(::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid, :Euclid, :Circular, :Euclid, :Euclid)
+getManifolds(::InstanceType{typeof(AMP.SE3_Manifold)}) = (:Euclid, :Euclid, :Euclid, :Circular, :Circular, :Circular)
+
+# legacy support, will be deprecated
+Base.convert(::Type{<:Tuple}, mani::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
+
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Point2}) = AMP.Euclid2
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Point3}) = AMP.Euclid3
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{DynPoint2}) = AMP.Euclid4
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose2}) = AMP.SE2_Manifold
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose3}) = AMP.SE3_Manifold
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{DynPose2}) = SE2E2_Manifold
+
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Point2Point2}) = AMP.Euclid2
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose2Point2}) = AMP.Euclid2
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose2Point2Bearing}) = AMP.Euclid
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Point2Point2Range}) = AMP.Euclid
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose2Point2Range}) = AMP.Euclid
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose2Point2BearingRange}) = AMP.Euclid2
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose2Pose2}) = AMP.SE2_Manifold
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{Pose3Pose3}) = AMP.SE3_Manifold
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{DynPoint2DynPoint2}) = AMP.Euclid4
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{DynPose2DynPose2}) = SE2E2_Manifold
+Base.convert(::Type{<:ManifoldsBase.Manifold}, ::InstanceType{VelPose2VelPose2}) = SE2E2_Manifold
 
 
 

src/RoME.jl

@@ -22,12 +22,15 @@ using
   TensorCast,
   ManifoldsBase
 
+# to avoid name conflicts
+import Manifolds: SpecialEuclidean, ProductRepr
+
 # using Graphs,  # TODO determine how many parts still require Graphs still directly
 
 
 import Base: +, \, convert
 import TransformUtils: ⊖, ⊕, convert, compare, ominus, veeQuaternion
-import IncrementalInference: convert, getSample, reshapeVec2Mat, extractdistribution, DFG, getManifolds
+import IncrementalInference: convert, getSample, reshapeVec2Mat, DFG, getManifolds
 # not sure why this is gives import error
 import DistributedFactorGraphs: compare
 import DistributedFactorGraphs: getDimension, getManifolds

src/factors/DynPose2D.jl

@@ -3,6 +3,52 @@
 export   DynPose2, DynPose2VelocityPrior, PackedDynPose2VelocityPrior, DynPose2Pose2, PackedDynPose2Pose2
 
 
+
+## ==================================================================================================
+## Hack to be removed or updated
+## FIXME ME ON FIRE use the ManifoldsBase.jl prescribed interface method instead:
+##   https://juliamanifolds.github.io/Manifolds.jl/stable/examples/manifold.html#manifold-tutorial
+## ==================================================================================================
+
+
+import ApproxManifoldProducts: coords, uncoords, getPointsManifold, _makeVectorManifold
+
+export SE2E2_Manifold
+
+
+# this is a hack and not fully implemented as per: 
+struct _SE2E2 <: ManifoldsBase.Manifold{ManifoldsBase.ℝ} end
+
+const SE2E2_Manifold = _SE2E2()
+
+AMP.coords(::Type{<:typeof(SE2E2_Manifold)}, p::ProductRepr) = [p.parts[1][1], p.parts[1][2], atan(p.parts[2][2,1],p.parts[2][1,1]), p.parts[3][1], p.parts[3][2]]
+
+function AMP.uncoords(::typeof(SE2E2_Manifold), p::AbstractVector{<:Real})
+  α = p[3]
+  ProductRepr(([p[1], p[2]]), [cos(α) -sin(α); sin(α) cos(α)], ([p[4], p[5]]))
+end
+
+function AMP.getPointsManifold(mkd::ManifoldKernelDensity{M}) where {M <: typeof(SE2E2_Manifold)}
+  data_ = getPoints(mkd.belief)
+  [uncoords(mkd.manifold, view(data_, :, i)) for i in 1:size(data_,2)]
+end
+
+function Statistics.mean(::typeof(SE2E2_Manifold), pts::AbstractVector)
+  se2_ = (d->ProductRepr(d.parts[1], d.parts[2])).(pts)
+  mse2 = mean(SpecialEuclidean(2), se2_)
+  e2_ = (d->ProductRepr(d.parts[3])).(pts)
+  me2 = mean(Euclidean(2), e2_)
+  ProductRepr(mse2.parts[1], mse2.parts[2], me2.parts[1])
+end
+
+AMP._makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(SE2E2_Manifold)} = coords(M, prr)
+
+
+
+## ==================================================================================================
+
+
+
 """
 $(TYPEDEF)
 """

src/factors/Pose2D.jl

@@ -1,4 +1,7 @@
 
+export SpecialEuclidean
+
+
 """
 $(TYPEDEF)
 

test/testBearingRange2D.jl

@@ -18,7 +18,7 @@ addVariable!(fg, :x0, Pose2)
 addVariable!(fg, :x1, Point2)
 addFactor!(fg, [:x0;:x1], p2br, graphinit=false)
 
-meas = freshSamples(IIF._getCCW(fg, :x0x1f1), 100)
+meas = sampleFactor(IIF._getCCW(fg, :x0x1f1), 100)
 
 ##
 

test/testInflation380.jl

@@ -5,7 +5,7 @@
 using Test
 using RoME
 using Statistics
-
+using Manifolds
 
 ##
 
@@ -197,13 +197,17 @@ IIF.solveFactorGraphParametric!(fg)
 @show getPPE(fg, :x2, :parametric).suggested
 @show getPPE(fg, :x2, :default).suggested
 
-test_err = getPPE(fg, :x2, :default).suggested - getPPE(fg, :x2, :parametric).suggested
-# arg, workaround until #244
-theta = (getBelief(fg, :x2, :default) |> getPoints)[3,:] .+ pi
-theta .= TU.wrapRad.(theta)
-@show theta_ = Statistics.mean(theta)
-@show theta_ref = TU.wrapRad(getPPE(fg, :x2, :parametric).suggested[3] + pi)
-test_err[3] = theta_ref - theta_
+test_err = 9999*ones(3)
+test_err[1:2] = getPPE(fg, :x2, :default).suggested[1:2] - getPPE(fg, :x2, :parametric).suggested[1:2]
+@show theta     = getPPE(fg, :x2, :default).suggested[3]
+@show theta_ref = getPPE(fg, :x2, :parametric).suggested[3]
+
+# # arg, workaround until #244
+# theta = (getBelief(fg, :x2, :default) |> getPoints)[3,:] .+ pi
+# theta .= TU.wrapRad.(theta)
+# @show theta_ = Statistics.mean(theta)
+# @show theta_ref = TU.wrapRad(getPPE(fg, :x2, :parametric).suggested[3] + pi)
+@show test_err[3] = Manifolds.log(Manifolds.Circle(), theta_ref, theta) # theta_ref - theta_
 @show test_err .= abs.(test_err)
 
 @test isapprox(test_err[1], 0, atol=0.5)
@@ -221,7 +225,7 @@ end
 # using RoMEPlotting
 # Gadfly.set_default_plot_size(25cm,20cm)
 
-# ##
+##
 
 # pl1 = plotSLAM2D(fg, solveKey=:default, drawPoints=true, drawEllipse=true, drawContour=false, xmin=-2.5,xmax=0,ymin=-1.5,ymax=1.5)
 # pl2 = plotSLAM2D(fg, solveKey=:parametric, drawPoints=false, drawContour=false, xmin=-2.5,xmax=0,ymin=-1.5,ymax=1.5)

test/testPose3Pose3NH.jl

@@ -91,6 +91,9 @@ global X3pts = approxConv(fg, :x2x3f1, :x3)
 global X2ptsMean = Statistics.mean(X2pts,dims=2)
 global X3ptsMean = Statistics.mean(X3pts,dims=2)
 
+@show X2ptsMean
+@show X3ptsMean
+
 @test  sum(map(Int, abs.(X2ptsMean) - [25.0;0;0;0;0;0] .< 5.0 ))  == 6
 @test  sum(map(Int, abs.(X3ptsMean -  [50.0;0;0;0;0;0]) .< 5.0 )) == 6
 

test/testhigherdimroots.jl

@@ -31,7 +31,7 @@ function (cfo::CalcFactor{<:RotationTest})( meas,
   q12 = q1*q_conj(q2)
   qq = dq*q_conj(q12)
   @show res
-  vee!(res, convert(TU.so3, qq))
+  TransformUtils.vee!(res, convert(TU.so3, qq))
   return res
 end
 

test/testpartialpose3.jl

@@ -123,7 +123,7 @@ addFactor!(tfg, [:x0;:x1], xyy, graphinit=false)
 
 # meas = getSample(xyy,100)
 ccw = IIF._getCCW(tfg, :x0x1f1)
-meas = freshSamples(ccw, 100)
+meas = sampleFactor(ccw, 100)
 
 @test norm(Statistics.std(meas[1],dims=2) - [0.01;0.01;0.002]) < 0.05