From 2f53d16ba0bd4d03bcdb86ccd18ee00deef14aa3 Mon Sep 17 00:00:00 2001
From: dehann <dehann.fourie@gmail.com>
Date: Mon, 27 Mar 2023 00:38:11 -0700
Subject: [PATCH] wip on Position3h

---
 src/RoME.jl                    |  4 +++-
 src/factors/Range2D.jl         |  8 ++++----
 src/services/FixmeManifolds.jl |  4 ++--
 src/variables/VariableTypes.jl | 21 +++++++++++++++++++--
 4 files changed, 28 insertions(+), 9 deletions(-)

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)