update to new defVariable

JuliaRobotics · Mar 28, 2021 · e44fbb1 · e44fbb1
1 parent bc29a5f
commit e44fbb1
Show file tree

Hide file tree

Showing 7 changed files with 111 additions and 70 deletions.
diff --git a/Project.toml b/Project.toml
@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.14.1"
+version = "0.15.0"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -33,11 +33,11 @@ TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 [compat]
 ApproxManifoldProducts = "0.3.1"
 CoordinateTransformations = "0.5, 0.6, 0.7"
-DistributedFactorGraphs = "0.12"
+DistributedFactorGraphs = "0.13"
 Distributions = "0.21, 0.22, 0.23, 0.24"
 DocStringExtensions = "0.7, 0.8"
 FileIO = "1.0.2, 1.1, 1.2"
-IncrementalInference = "0.21.2, 0.22"
+IncrementalInference = "0.23"
 JLD2 = "0.2, 0.3, 0.4"
 KernelDensityEstimate = "0.5.1, 0.6"
 Manifolds = "0.4.19"

diff --git a/src/Deprecated.jl b/src/Deprecated.jl
@@ -16,16 +16,17 @@ getManifolds(::InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Cir
 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{<:Tuple}, mani::InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
+Base.convert(::Type{<:Tuple}, mani::InstanceType{typeof(BearingRange_Manifold)}) = (:Circular,:Euclid)
+
+# Variables dont need to re-overload these functions from @defVariable (factors dont have easy macro yet)
+# 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

diff --git a/src/RoME.jl b/src/RoME.jl
@@ -244,6 +244,7 @@ include("SpecialDefinitions.jl")
 # include("variables/Pose3D.jl")
 
 #uses DFG v0.10.2 @defVariable for above
+include("variables/Local_Manifold_Workaround.jl")
 include("variables/VariableTypes.jl")
 
 ## More factor types

diff --git a/src/TemporaryFunctionality.jl b/src/TemporaryFunctionality.jl
@@ -3,13 +3,6 @@ import IncrementalInference: selectFactorType, getDomain
 
 export getDomain
 
-## =============================================================================
-## Needs a home
-## =============================================================================
-
-# Still experimental
-# export BearingRange2
-@defVariable BearingRange2 2 (:Circular, :Euclid)
 
 ## ============================================================================
 # Starting integration with Manifolds.jl, via ApproxManifoldProducts.jl first

diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl
@@ -4,50 +4,6 @@ export   DynPose2, DynPose2VelocityPrior, PackedDynPose2VelocityPrior, DynPose2P
 
 
 
-## ==================================================================================================
-## 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/variables/Local_Manifold_Workaround.jl b/src/variables/Local_Manifold_Workaround.jl
@@ -0,0 +1,82 @@
+## ==================================================================================================
+## 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()
+
+ManifoldsBase.manifold_dimension(::_SE2E2) = 5
+
+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)
+
+
+
+
+## =============================================================================
+## Needs a home
+## =============================================================================
+
+export BearingRange_Manifold
+
+
+struct _CircleEuclid <: ManifoldsBase.Manifold{ManifoldsBase.ℝ} end
+
+ManifoldsBase.manifold_dimension(::_CircleEuclid) = 2
+
+const BearingRange_Manifold = _CircleEuclid()
+
+AMP.coords(::Type{<:typeof(BearingRange_Manifold)}, p::ProductRepr) = [p.parts[1][1]; p.parts[2][1]]
+
+function AMP.uncoords(::typeof(BearingRange_Manifold), p::AbstractVector{<:Real})
+  ProductRepr(([p[1];]), ([p[2];]))
+end
+
+function AMP.getPointsManifold(mkd::ManifoldKernelDensity{M}) where {M <: typeof(BearingRange_Manifold)}
+  data_ = getPoints(mkd.belief)
+  [uncoords(mkd.manifold, view(data_, :, i)) for i in 1:size(data_,2)]
+end
+
+function Statistics.mean(::typeof(BearingRange_Manifold), pts::AbstractVector)
+  TensorCast.@cast bearing[i] := pts[i][1]
+  TensorCast.@cast range_[i] := pts[i][2]
+  mc = mean(Circle(), bearing)
+  mr = mean(range_)
+
+  return [mc; mr]
+end
+
+AMP._makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(BearingRange_Manifold)} = coords(M, prr)
+
+
+
+## ==================================================================================================
diff --git a/src/variables/VariableTypes.jl b/src/variables/VariableTypes.jl
@@ -2,12 +2,13 @@
 
 export projectCartesian
 
+
 """
 $(TYPEDEF)
 
 XY Euclidean manifold variable node softtype.
 """
-@defVariable Point2 2 (:Euclid, :Euclid)
+@defVariable Point2 Euclidean(2) # 2 (:Euclid, :Euclid)
 
 
 """
@@ -21,15 +22,15 @@ Example
 p3 = Point3()
 ```
 """
-@defVariable Point3 3 (:Euclid,:Euclid,:Euclid)
+@defVariable Point3 Euclidean(3) # 3 (:Euclid,:Euclid,:Euclid)
 
 
 """
 $(TYPEDEF)
 
 Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM.
 """
-@defVariable Pose2 3 (:Euclid,:Euclid,:Circular)
+@defVariable Pose2 SpecialEuclidean(2) # 3 (:Euclid,:Euclid,:Circular)
 
 """
 $(TYPEDEF)
@@ -41,22 +42,25 @@ Future:
 - Work in progress on AMP3D for proper non-Euler angle on-manifold operations.
 - TODO the AMP upgrade is aimed at resolving 3D to Quat/SE3/SP3 -- current Euler angles will be replaced
 """
-@defVariable Pose3 6 (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular)
+@defVariable Pose3 SpecialEuclidean(3) # 6 (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular)
 
 """
 $(TYPEDEF)
 
 Dynamic point in 2D space with velocity components: `x, y, dx/dt, dy/dt`
 
 """
-@defVariable DynPoint2 4 (:Euclid,:Euclid,:Euclid,:Euclid)
+@defVariable DynPoint2 Euclidean(4) # 4 (:Euclid,:Euclid,:Euclid,:Euclid)
 
 """
 $(TYPEDEF)
 
 Dynamic pose variable with velocity components: `x, y, theta, dx/dt, dy/dt`
+
+Note
+- The `SE2E2_Manifold` definition used currently is a hack to simplify the transition to Manifolds.jl, see #244 
 """
-@defVariable DynPose2 5 (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
+@defVariable DynPose2 SE2E2_Manifold # 5 (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
 
 
 
@@ -65,13 +69,17 @@ $SIGNATURES
 
 Function to project only XY data onto Cartesian plane for 2D plotting.
 """
-projectCartesian(pose::Union{Point2,Point3, Pose2, Pose3, DynPoint2, DynPose2}, 
+projectCartesian(pose::Union{<:Point2,<:Point3,<:Pose2,<:Pose3,<:DynPoint2,<:DynPose2}, 
                  x::Vector{Float64}) = [x[1]; x[2]; 0]
 
 #
 
 
 
+# Still experimental
+# export BearingRange2
+@defVariable BearingRange2 BearingRange_Manifold # 2 (:Circular, :Euclid)
+