diff --git a/Project.toml b/Project.toml index 527477bf..a170531d 100644 --- 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 index dbe9d9d1..fd031096 100644 --- 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 index b2c9e36e..51087911 100644 --- 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 index 45b98439..7386487b 100644 --- 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 index f8ee878c..89184dbb 100644 --- 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 new file mode 100644 index 00000000..adccdac4 --- /dev/null +++ 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 index a145b736..917b6e0e 100644 --- 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,7 +22,7 @@ Example p3 = Point3() ``` """ -@defVariable Point3 3 (:Euclid,:Euclid,:Euclid) +@defVariable Point3 Euclidean(3) # 3 (:Euclid,:Euclid,:Euclid) """ @@ -29,7 +30,7 @@ $(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,7 +42,7 @@ 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) @@ -49,14 +50,17 @@ $(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) +