From ebc22b92dffecc281a064e6975345905ae39d085 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 26 Mar 2021 14:45:40 -0400 Subject: [PATCH 1/5] AMP v0.3, towards Manifolds.jl --- Project.toml | 6 +++-- src/Deprecated.jl | 53 +++++++++++++++++++++++--------------- src/RoME.jl | 5 +++- src/factors/DynPose2D.jl | 46 +++++++++++++++++++++++++++++++++ src/factors/Pose2D.jl | 3 +++ test/testBearingRange2D.jl | 2 +- test/testInflation380.jl | 22 +++++++++------- test/testhigherdimroots.jl | 2 +- test/testpartialpose3.jl | 2 +- 9 files changed, 105 insertions(+), 36 deletions(-) diff --git a/Project.toml b/Project.toml index c55c205c..41529fbb 100644 --- a/Project.toml +++ b/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" 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" diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 4ca8ea40..c1783c48 100644 --- a/src/Deprecated.jl +++ b/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 diff --git a/src/RoME.jl b/src/RoME.jl index 08ca88d2..b2c9e36e 100644 --- a/src/RoME.jl +++ b/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 diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl index 33fb3afe..f8ee878c 100644 --- a/src/factors/DynPose2D.jl +++ b/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) """ diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 440d6b6c..59a02a61 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -1,4 +1,7 @@ +export SpecialEuclidean + + """ $(TYPEDEF) diff --git a/test/testBearingRange2D.jl b/test/testBearingRange2D.jl index c517c6dc..699faa36 100644 --- a/test/testBearingRange2D.jl +++ b/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) ## diff --git a/test/testInflation380.jl b/test/testInflation380.jl index 54853358..beba9366 100644 --- a/test/testInflation380.jl +++ b/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) diff --git a/test/testhigherdimroots.jl b/test/testhigherdimroots.jl index 4215f48e..39bc4823 100644 --- a/test/testhigherdimroots.jl +++ b/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 diff --git a/test/testpartialpose3.jl b/test/testpartialpose3.jl index 241cf65b..9de789ae 100644 --- a/test/testpartialpose3.jl +++ b/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 From 602618a63f53e515df740405d0216b4bc8913eaa Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 26 Mar 2021 19:01:46 -0400 Subject: [PATCH 2/5] update CI --- .github/workflows/ci.yml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5c01aa87..b1a4efd4 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -14,8 +14,7 @@ jobs: fail-fast: false matrix: version: - - '1.5' - - '^1.6.0-0' + - '1.6' os: - ubuntu-latest arch: @@ -56,7 +55,7 @@ jobs: - uses: julia-actions/setup-julia@v1 with: - version: 1.5 + version: 1.6 arch: x64 - uses: actions/cache@v1 From 6129ceddbcd25361f8d4eb021a7a23f408ff8106 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 26 Mar 2021 19:04:24 -0400 Subject: [PATCH 3/5] add nightly --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b1a4efd4..2e3c4de7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -15,6 +15,7 @@ jobs: matrix: version: - '1.6' + - 'nightly' os: - ubuntu-latest arch: From 18dfffe142064ba8d6b6adccd0f71fa91430fb75 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 26 Mar 2021 19:59:00 -0400 Subject: [PATCH 4/5] need copy on ProductRepr --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 41529fbb..bea9b227 100644 --- a/Project.toml +++ b/Project.toml @@ -40,7 +40,7 @@ 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" +Manifolds = "0.4.19" ManifoldsBase = "0.10" Optim = "0.22, 1.0" ProgressMeter = "1" From 1604521c5e10362e71d04ea9e7ef4ec67620cdc1 Mon Sep 17 00:00:00 2001 From: dehann Date: Fri, 26 Mar 2021 23:36:01 -0400 Subject: [PATCH 5/5] print in test --- test/testPose3Pose3NH.jl | 3 +++ 1 file changed, 3 insertions(+) diff --git a/test/testPose3Pose3NH.jl b/test/testPose3Pose3NH.jl index 594874b0..89e90483 100644 --- a/test/testPose3Pose3NH.jl +++ b/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