Update Pose2 to LieGroups

Author: Affie

.gitignore

@@ -1,4 +1,4 @@
-/dev
+dev
 Manifest.toml
 *.so
 *.cov

Project.toml

@@ -18,6 +18,7 @@ ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534"
 IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480"
 Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59"
 KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d"
+LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
 ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
@@ -65,6 +66,7 @@ ImageIO = "0.6"
 IncrementalInference = "0.35"
 Interpolations = "0.14, 0.15"
 KernelDensityEstimate = "0.5.1, 0.6"
+LieGroups = "0.1.1"
 LinearAlgebra = "1.10"
 Manifolds = "0.10.1"
 ManifoldsBase = "0.15, 1"

src/RoME.jl

@@ -21,7 +21,8 @@ using
   DistributedFactorGraphs,
   TensorCast,
   ManifoldsBase,
-  Manifolds
+  Manifolds,
+  LieGroups
 
 using StaticArrays
 using PrecompileTools
@@ -33,7 +34,7 @@ using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, SpecialOr
 import Manifolds: project, project!, identity_element
 
 import Rotations as _Rot
-import Rotations: ⊕, ⊖ # TODO deprecate
+# import Rotations: ⊕, ⊖ # TODO deprecate
 
 export SpecialOrthogonal, SpecialEuclidean
 export submanifold_component

src/factors/Inertial/IMUDeltaFactor.jl

@@ -394,7 +394,7 @@ function (cf::CalcFactor{<:IMUDeltaFactor})(
     q_SE3,
     q_vel,
     b = zeros(SVector{6,Float64})
-) where T <: Real
+)
     p = ArrayPartition(p_SE3.x[2], p_vel, p_SE3.x[1])
     q = ArrayPartition(q_SE3.x[2], q_vel, q_SE3.x[1])
     return cf(Δmeas, p, q, b)

src/factors/Pose2D.jl

@@ -8,16 +8,15 @@ Rigid transform between two Pose2's, assuming (x,y,theta).
 Calcuated as:
 ```math
 \\begin{aligned}
-\\hat{q}=\\exp_pX_m\\\\
-X = \\log_q \\hat{q}\\\\
-X^i = \\mathrm{vee}(q, X)
+\\hat{X}_p=\\log_pq\\\\
+X^i = \\mathrm{vee}(X_m-\\hat{X})
 \\end{aligned}
 ```
 with:
 ``\\mathcal M= \\mathrm{SE}(2)`` Special Euclidean group\\
 ``p`` and ``q`` ``\\in \\mathcal M`` the two Pose2 points\\
 the measurement vector ``X_m \\in T_p \\mathcal M``\\
-and the error vector ``X \\in T_q \\mathcal M``\\
+and the error vector ``\hat{X} \\in T_p \\mathcal M``\\
 ``X^i`` coordinates of ``X``
 
 DevNotes
@@ -27,53 +26,27 @@ Related
 
 [`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref)
 """
-Base.@kwdef struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
+Base.@kwdef struct Pose2Pose2{T<:IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
   Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0]))
 end
 
 DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation())
 
 Pose2Pose2(::UniformScaling) = Pose2Pose2()
 
-
-# Assumes X is a tangent vector
-function (cf::CalcFactor{<:Pose2Pose2})(_X::AbstractArray{MT}, _p::AbstractArray{PT}, _q::AbstractArray{LT})  where {MT,PT,LT}
-  #TODO remove this convertions
-  # @warn "This warning should not be triggered after StaticArrays upgrade" maxlog=10
-  T = promote_type(MT, PT, LT)
-  X = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _X)
-  p = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _p)
-  q = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _q)
-  return cf(X,p,q)
-end
-
-# function calcPose2Pose2(
-function (cf::CalcFactor{<:Pose2Pose2})(
-              X::ArrayPartition{XT, Tuple{SVector{2, XT}, SMatrix{2, 2, XT, 4}}},
-              p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, 
-              q::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where {XT<:Real,T<:Real}
-
-    M = getManifold(Pose2)
-    # ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I))
-    ϵ0 = getPointIdentity(M)
-
-    ϵX = exp(M, ϵ0, X)
-    # q̂ = Manifolds.compose(M, p, ϵX)    
-    q̂ = _compose(M, p, ϵX)
-    X_hat = log(M, q, q̂)#::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}
-    # Xc = vee(M, q, X_hat)
-    Xc = _vee(M, X_hat)#::SVector{3,T}
-    return Xc
+function (cf::CalcFactor{<:Pose2Pose2})(X, p, q)
+  G = getManifold(Pose2)
+  X̂ = log(base_manifold(G), getPointIdentity(G), compose(G, inv(G, p), q))
+  return vee(LieAlgebra(G), X - X̂)
 end
 
-
 # NOTE, serialization support -- will be reduced to macro in future
 # ------------------------------------
 
 """
 $(TYPEDEF)
 """
-Base.@kwdef struct PackedPose2Pose2  <: AbstractPackedFactor
+Base.@kwdef struct PackedPose2Pose2 <: AbstractPackedFactor
   Z::PackedSamplableBelief
 end
 function convert(::Type{Pose2Pose2}, d::PackedPose2Pose2)
@@ -85,7 +58,7 @@ end
 
 
 # FIXME, rather have separate compareDensity functions
-function compare(a::Pose2Pose2,b::Pose2Pose2; tol::Float64=1e-10)
+function compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64=1e-10)
   return compareDensity(a.Z, b.Z)
 end
 

src/variables/VariableTypes.jl

@@ -32,7 +32,7 @@ $(TYPEDEF)
 
 Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM.
 """
-@defVariable Pose2 SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0])
+@defVariable Pose2 SpecialEuclideanGroup(2; variant=:right) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0])
 
 """
 $(TYPEDEF)