wip on Position3h

JuliaRobotics · Mar 27, 2023 · 2f53d16 · 2f53d16
1 parent 688cf54
commit 2f53d16
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 9 deletions.
diff --git 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
@@ -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
@@ -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
@@ -8,15 +8,15 @@ export projectCartesian
 """
 $(TYPEDEF)
 
-XY Euclidean manifold variable node softtype.
+XY Euclidean manifold variable type.
 """
 @defVariable Point2 TranslationGroup(2) [0.0;0.0]
 
 
 """
 $(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)