diff --git a/src/RoME.jl b/src/RoME.jl index be8e0fb8..91c79e27 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -27,7 +27,9 @@ using SnoopPrecompile # to avoid name conflicts import Manifolds -using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, ProductRepr, SpecialOrthogonal, TranslationGroup, identity_element, submanifold_component, Identity, affine_matrix +using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean +using Manifolds: ProjectiveSpace, SpecialOrthogonal, TranslationGroup +using Manifolds: identity_element, submanifold_component, Identity, affine_matrix import Manifolds: project, project!, identity_element diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl index c54788a3..02ddb9fc 100644 --- a/src/factors/Range2D.jl +++ b/src/factors/Range2D.jl @@ -52,10 +52,10 @@ function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi::Manifolds.ArrayPartition # Basically `EuclidDistance` return rho .- norm(lm .- xi.x[1]) end -function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi::ProductRepr, lm) - # Basically `EuclidDistance` - return rho .- norm(lm .- xi.parts[1]) -end +# function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi::ProductRepr, lm) +# # Basically `EuclidDistance` +# return rho .- norm(lm .- xi.parts[1]) +# end Base.@kwdef struct PackedPose2Point2Range <: AbstractPackedFactor diff --git a/src/services/FixmeManifolds.jl b/src/services/FixmeManifolds.jl index f302a644..36c1de8f 100644 --- a/src/services/FixmeManifolds.jl +++ b/src/services/FixmeManifolds.jl @@ -17,7 +17,7 @@ const SE2E2_Manifold = _SE2E2() MB.manifold_dimension(::_SE2E2) = 5 -AMP.makeCoordsFromPoint(::Type{<:typeof(SE2E2_Manifold)}, p::Manifolds.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]] +AMP.makeCoordsFromPoint(::Type{<:typeof(SE2E2_Manifold)}, p::ArrayPartition) = [p.x[1][1], p.x[1][2], atan(p.x[2][2,1],p.x[2][1,1]), p.x[3][1], p.x[3][2]] function AMP.makePointFromCoords(::typeof(SE2E2_Manifold), p::AbstractVector{<:Real}) α = p[3] @@ -56,7 +56,7 @@ MB.manifold_dimension(::_CircleEuclid) = 2 const BearingRange_Manifold = _CircleEuclid() # MB.manifold_dimension(::BearingRange_Manifold) = 2 -AMP.makeCoordsFromPoint(::Type{<:typeof(BearingRange_Manifold)}, p::ProductRepr) = [p.parts[1][1]; p.parts[2][1]] +AMP.makeCoordsFromPoint(::Type{<:typeof(BearingRange_Manifold)}, p::ArrayPartition) = [p.x[1][1]; p.x[2][1]] function AMP.makePointFromCoords(::typeof(BearingRange_Manifold), p::AbstractVector{<:Real}) ArrayPartition(([p[1];]), ([p[2];])) diff --git a/src/variables/VariableTypes.jl b/src/variables/VariableTypes.jl index 34479d3c..539f88b1 100644 --- a/src/variables/VariableTypes.jl +++ b/src/variables/VariableTypes.jl @@ -8,7 +8,7 @@ export projectCartesian """ $(TYPEDEF) -XY Euclidean manifold variable node softtype. +XY Euclidean manifold variable type. """ @defVariable Point2 TranslationGroup(2) [0.0;0.0] @@ -16,7 +16,7 @@ XY Euclidean manifold variable node softtype. """ $(TYPEDEF) -XYZ Euclidean manifold variable node softtype. +XYZ Euclidean manifold variable type. Example ------- @@ -27,6 +27,23 @@ p3 = Point3() @defVariable Point3 TranslationGroup(3) [0;0;0.0] +""" +$(TYPEDEF) + +Homogeneous XYZ projective manifold variable type. + +NOTE, NOT WORKING AS A GROUP SINCE RECONCILIATION WITH MANIFOLDS EXMAP STILL TBD +- see https://github.com/JuliaRobotics/RoME.jl/issues/661 + +Example +------- +```julia +p3 = Point3() +``` +""" +@defVariable Point3h ProjectiveSpace(3) MVector{4,Float64}(0,0,0,1.) + + """ $(TYPEDEF)