Merge pull request #643 from JuliaRobotics/22Q4/enh/partialmanifolds

better standardize partials and depr threadmodel

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.21.1"
+version = "0.22.0"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -34,13 +34,13 @@ TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b"
 TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63"
 
 [compat]
-ApproxManifoldProducts = "0.6"
+ApproxManifoldProducts = "0.6.2"
 CoordinateTransformations = "0.5, 0.6"
 DistributedFactorGraphs = "0.18.4"
 Distributions = "0.24, 0.25"
 DocStringExtensions = "0.8, 0.9"
 FileIO = "1"
-IncrementalInference = "0.31"
+IncrementalInference = "0.32"
 JLD2 = "0.3, 0.4"
 KernelDensityEstimate = "0.5.1, 0.6"
 Manifolds = "0.8.11"

attic/FluxModelsPose2Pose2.jl

@@ -161,8 +161,8 @@ function (cfo::CalcFactor{<:FluxModelsPose2Pose2})(meas1,meas2,meas3,Xi,Xj)
   end
 
   # calculate the error for that measurement sample as Pose2Pose2
-  #TODO
-  cfZij = CalcFactor( nfb.Zij, nothing, 0, 0, (), [])
+  # FIXME, not sure this constructor still works
+  cfZij = CalcFactor( nfb.Zij, nothing, 0, (), [])
   res = cfZij((meas[1],), Xi, Xj)
 
 end

src/Deprecated.jl

@@ -3,46 +3,46 @@
 ## Legacy, remove some time after RoME v0.22
 ##==============================================================================
 
-@deprecate homographyToCoordinates(w...;kw...) homography_to_coordinates(w...;kw...)
-
-export measureMeanDist
-function measureMeanDist(fg::AbstractDFG, a::AbstractString, b::AbstractString)
-  @error "RoME.measureMeanDist is obsolete"
-  #bearrang!(residual::Array{Float64,1}, Z::Array{Float64,1}, X::Array{Float64,1}, L::Array{Float64,1})
-  res = zeros(2)
-  A = getVal(fg,a)
-  B = getVal(fg,b)
-  Ax = Statistics.mean(vec(A[1,:]))
-  Ay = Statistics.mean(vec(A[2,:]))
-  Bx = Statistics.mean(vec(B[1,:]))
-  By = Statistics.mean(vec(B[2,:]))
-  dx = Bx - Ax
-  dy = By - Ay
-  b = atan(dy,dx)
-  r = sqrt(dx^2 + dy^2)
-  return r, b
-end
-
-# should be deprecated or indicated more clearly
-@deprecate lsrBR(a) [a[2,:];a[1,:]]'
-
-# import AMP: _makeVectorManifold
-# AMP._makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(BearingRange_Manifold)} = coords(M, prr)
+# @deprecate homographyToCoordinates(w...;kw...) homography_to_coordinates(w...;kw...)
+
+# export measureMeanDist
+# function measureMeanDist(fg::AbstractDFG, a::AbstractString, b::AbstractString)
+#   @error "RoME.measureMeanDist is obsolete"
+#   #bearrang!(residual::Array{Float64,1}, Z::Array{Float64,1}, X::Array{Float64,1}, L::Array{Float64,1})
+#   res = zeros(2)
+#   A = getVal(fg,a)
+#   B = getVal(fg,b)
+#   Ax = Statistics.mean(vec(A[1,:]))
+#   Ay = Statistics.mean(vec(A[2,:]))
+#   Bx = Statistics.mean(vec(B[1,:]))
+#   By = Statistics.mean(vec(B[2,:]))
+#   dx = Bx - Ax
+#   dy = By - Ay
+#   b = atan(dy,dx)
+#   r = sqrt(dx^2 + dy^2)
+#   return r, b
+# end
+
+# # should be deprecated or indicated more clearly
+# @deprecate lsrBR(a) [a[2,:];a[1,:]]'
+
+# # import AMP: _makeVectorManifold
+# # AMP._makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(BearingRange_Manifold)} = coords(M, prr)
 
 
 
 ##==============================================================================
 ## Remove as part of Manifolds.jl consolidation, #244
 ##==============================================================================
 
-export veePose3, veePose
+# export veePose3, veePose
 
-function veePose3(s::SE3)
-  TransformUtils.veeEuler(s)
-end
-function veePose(s::SE3)
-  TransformUtils.veeEuler(s)
-end
+# function veePose3(s::SE3)
+#   TransformUtils.veeEuler(s)
+# end
+# function veePose(s::SE3)
+#   TransformUtils.veeEuler(s)
+# end
 
 
 # legacy support, will be deprecated
@@ -53,14 +53,14 @@ Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(BearingRange_Manifold)})
 Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(Manifolds.ProductGroup(ProductManifold(SpecialEuclidean(2), TranslationGroup(2))))}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid)
 
 
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Point2Point2}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2Bearing}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Point2Point2Range}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2Range}) = AMP.Euclid
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2BearingRange}) = AMP.Euclid2
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Pose2}) = AMP.SE2_Manifold
-Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose3Pose3}) = AMP.SE3_Manifold
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Point2Point2}) = AMP.Euclid2
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2}) = AMP.Euclid2
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2Bearing}) = AMP.Euclid
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Point2Point2Range}) = AMP.Euclid
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2Range}) = AMP.Euclid
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Point2BearingRange}) = AMP.Euclid2
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose2Pose2}) = AMP.SE2_Manifold
+# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Pose3Pose3}) = AMP.SE3_Manifold
 Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{DynPoint2DynPoint2}) = AMP.Euclid4
 Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{DynPose2DynPose2}) = SE2E2_Manifold
 Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{VelPose2VelPose2}) = SE2E2_Manifold

src/RoME.jl

@@ -29,7 +29,7 @@ using SnoopPrecompile
 import Manifolds
 using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, ProductRepr, SpecialOrthogonal, TranslationGroup, identity_element, submanifold_component, Identity, affine_matrix
 
-import Manifolds: project, project!
+import Manifolds: project, project!, identity_element
 
 import Rotations as _Rot
 

src/factors/DynPose2D.jl

@@ -16,7 +16,7 @@ function getSample(cf::CalcFactor{<:DynPose2VelocityPrior})
   Zpose = cf.factor.Zpose
   Zvel = cf.factor.Zvel
   p = getPointIdentity(DynPose2())
-  M = getManifold(cf.factor)
+  M = cf.manifold # getManifold(cf.factor)
 
   Xc = [rand(Zpose);rand(Zvel)]
 
@@ -43,6 +43,7 @@ function IIF.getMeasurementParametric(s::DynPose2VelocityPrior{<:MvNormal, <:MvN
 end
 
 function (cf::CalcFactor{<:DynPose2VelocityPrior})(meas, X)
+  # FIXME update Manifolds.jl usage
   #pose part, reused from PriorPose2
   iXihat = SE2(meas[1:3]) \ SE2(X[1:3])	
   res_pose = se2vee(iXihat)	
@@ -69,6 +70,7 @@ function (cf::CalcFactor{<:DynPose2Pose2})( meas,
                                             wXi,
                                             wXj  )
   #
+  # FIXME update to Manifolds.jl usage
   # cf.factor.Zpose(res, meas, wXi, wXj)
   wXjhat = SE2(wXi)*SE2(meas)
   jXjhat = SE2(wXj) \ wXjhat
@@ -152,6 +154,8 @@ function (cf::CalcFactor{<:DynPose2DynPose2})(meas,
                                               wXi,
                                               wXj  )
   #
+  # FIXME update to Manifolds.jl usage
+
   # vp2vp2.Zpose(res, userdata, idx, meas, Xi, Xj)
     # wXjhat = SE2(wxi[1:3,idx])*SE2(meas[1][1:3,idx])
     # jXjhat = SE2(wxj[1:3,idx]) \ wXjhat

src/factors/MutablePose2Pose2.jl

@@ -27,7 +27,7 @@ Related
 Pose2Pose2, Pose3Pose3, InertialPose3, DynPose2Pose2, Point2Point2, VelPoint2VelPoint2
 """
 function (cf::CalcFactor{<:MutablePose2Pose2Gaussian})(X, p, q)
-  M = getManifold(Pose2)
+  M = cf.manifold # getManifold(Pose2)
   q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups
   #TODO allocalte for vee! see Manifolds #412, fix for AD
   Xc = zeros(3)

src/factors/PartialPose3.jl

@@ -17,19 +17,32 @@ end
 PriorPose3ZRP(z::SamplableBelief,rp::SamplableBelief) = PriorPose3ZRP(;z, rp)
 
 # TODO should be dim 3 manifold
-getManifold(::PriorPose3ZRP) = getManifold(Pose3) # SpecialEuclidean(3)
+getManifold(zrp::PriorPose3ZRP) = ProductManifold(TranslationGroup(1),RealCircleGroup(),RealCircleGroup())
+# DIDNT WORK YET, partials need more attention:  getManifoldPartial(getManifold(Pose3), [zrp.partial...;]) 
+
+Manifolds.identity_element(::typeof(ProductManifold(TranslationGroup(1),RealCircleGroup(),RealCircleGroup())),dummy=nothing) = ArrayPartition([0.], [0.], [0.])
 
 #FIXME update to also only one measurement
 function getSample(cf::CalcFactor{<:PriorPose3ZRP})
+  # working towards producing samples as a point on the manifold of getManifold(::PriorPose3ZRP)
+  Mf = getManifold(Pose3) # full Pose3 with partial logic
 
-  #Rotation part: roll and pitch
+  #Rotation part: from Euler roll and pitch
   r,p = rand(cf.factor.rp)
   R = _Rot.RotYX(p, r) #TODO confirm RotYX(p,r) or RotXY(r,p)
 
   # Translation part: Z
   T = [0; 0; rand(cf.factor.z)]
-
-  return ArrayPartition(T, R)
+  pt = ArrayPartition(T, R)
+
+  # FIXME, this is probably not quite right
+  # to Lie exponential parameterization, notice world reference
+  w_Cp = vee(Mf, Identity(Mf), log(Mf, Identity(Mf), pt))
+  return ArrayPartition(
+    [w_Cp[cf.factor.partial[1]]], 
+    [w_Cp[cf.factor.partial[2]]], 
+    [w_Cp[cf.factor.partial[3]]] 
+  )
 end
 
 

src/factors/PartialPriorPose2.jl

@@ -10,21 +10,22 @@ Base.@kwdef struct PartialPriorYawPose2{T <: IIF.SamplableBelief} <: IIF.Abstrac
 end
 PartialPriorYawPose2(Z::SamplableBelief) = PartialPriorYawPose2(;Z)
 
+getManifold(::PartialPriorYawPose2) = RealCircleGroup() # SpecialEuclidean(2)
+
 function getSample(cf::CalcFactor{<:PartialPriorYawPose2})
 
   Z = cf.factor.Z
-  M = getManifold(cf.factor)
-  p = getPointIdentity(M)
+  return rand(Z,1)
+  # M = getManifold(cf.factor)
+  # p = getPointIdentity(M)
 
-  Xc = [0,0,rand(Z)]
+  # Xc = [0,0,rand(Z)]
 
-  X = hat(M, p, Xc)
-  points = exp(M, p, X)
-  return points
+  # X = hat(M, p, Xc)
+  # points = exp(M, p, X)
+  # return points
 end
 
-getManifold(::PartialPriorYawPose2) = SpecialEuclidean(2)
-
 ## Serialization support
 
 """

src/factors/Pose2D.jl

@@ -31,13 +31,14 @@ Base.@kwdef struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldM
   Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0]))
 end
 
-DFG.getManifold(::InstanceType{Pose2Pose2}) = getManifold(Pose2) # Manifolds.SpecialEuclidean(2)
+DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2)
 
 Pose2Pose2(::UniformScaling) = Pose2Pose2()
 
 function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, pp::Pose2Pose2)
   M = getManifold(pp)
-  (;manifold=M, ϵ0=getPointIdentity(M), Xc=zeros(3), q̂=getPointIdentity(M))
+  (M, getPointIdentity(M), zeros(3), getPointIdentity(M))
+  # (;manifold=M, ϵ0=getPointIdentity(M), Xc=zeros(3), q̂=getPointIdentity(M))
 end
 
 # Assumes X is a tangent vector
@@ -55,7 +56,7 @@ function (cf::CalcFactor{<:Pose2Pose2})(
               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)
+    M = cf.manifold # getManifold(Pose2)
     ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I))
 
     ϵX = exp(M, ϵ0, X)

src/factors/Pose2Point2.jl

@@ -20,14 +20,23 @@ Pose2Point2(Z::SamplableBelief) = Pose2Point2(;Z)
 getManifold(::InstanceType{Pose2Point2}) = getManifold(Point2)
 
 # define the conditional probability constraint
-function (cfo::CalcFactor{<:Pose2Point2})(meas,
-                                          wXi,
-                                          wLj )
+function (cfo::CalcFactor{<:Pose2Point2})(p_Xpq,
+                                          w_T_p,
+                                          w_Tl_q )
   #
-  #FIXME upgrade to manifolds 
-  _wXi = getCoordinates(Pose2, wXi)
-  wLj_pred = SE2(_wXi)*SE2([meas;0.0])
-  return  wLj .- se2vee(wLj_pred)[1:2]
+  M = SpecialEuclidean(2)
+
+  p_T_qhat = ArrayPartition(SA[p_Xpq[1];p_Xpq[2]], SMatrix{2,2}([1 0; 0 1.]))
+  _w_T_p = ArrayPartition(SA[w_T_p.x[1]...], SMatrix{2,2}(w_T_p.x[2]))
+  w_H_qhat = affine_matrix(M, _w_T_p) * affine_matrix(M, p_T_qhat)
+  # Issue, this produces ComposedFunction which errors on not having field .x[1]
+  # w_T_qhat = compose(M, _w_T_p, p_T_qhat)
+  return w_Tl_q .- w_H_qhat[1:2,end]
+
+  # upgraded to manifolds 
+  # w_Cp = getCoordinates(Pose2, w_T_p)
+  # w_Tl_q_pred = SE2(w_Cp)*SE2([p_Xpq;0.0])
+  # return  w_Tl_q .- se2vee(w_Tl_q_pred)[1:2]
 end
 
 

src/factors/Pose3D.jl

@@ -13,7 +13,7 @@ end
 getManifold(::InstanceType{PriorPose3}) = getManifold(Pose3) # SpecialEuclidean(3)
 
 function (cf::CalcFactor{<:PriorPose3})(m, p)
-  M = getManifold(cf.factor)
+  M = cf.manifold # getManifold(cf.factor)
   Xc = vee(M, p, log(M, p, m))
   return Xc
 end

src/factors/Pose3Pose3.jl

@@ -15,7 +15,7 @@ getManifold(::InstanceType{Pose3Pose3}) = getManifold(Pose3) # Manifolds.Special
 Pose3Pose3(::UniformScaling) = Pose3Pose3()
 
 function (cf::CalcFactor{<:Pose3Pose3})(X, p, q)
-    M = getManifold(Pose3)
+    M = cf.manifold # getManifold(Pose3)
     q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups
     #TODO allocalte for vee! see Manifolds #412, fix for AD
     # Xc = zeros(6)

src/factors/PriorPose2.jl

@@ -38,7 +38,7 @@ function (cf::CalcFactor{<:PriorPose2})(
             m::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, 
             p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real
 
-  M = getManifold(Pose2)
+  M = cf.manifold # getManifold(Pose2)
   Xc = _vee(M, log(M, p, m))
   return Xc
 end

src/factors/flux/MixtureFluxPose2Pose2.jl

@@ -31,7 +31,7 @@ function IIF.getSample(cfo::CalcFactor{<:MixtureFluxPose2Pose2})
     nfb.DT[] = (getTimestamp(cfo.fullvariables[2]) - getTimestamp(cfo.fullvariables[1])).value * 1e-3
   end
 
-  cf_ = CalcFactor( cfo.factor.Zij, 0, length(cfo._legacyMeas), cfo._legacyMeas, cfo._legacyParams, cfo.cache, cfo.fullvariables, cfo.solvefor)
+  cf_ = CalcFactor( cfo.factor.Zij, 0, cfo._legacyParams, cfo.cache, cfo.fullvariables, cfo.solvefor, cfo.manifold)
 
   smpl = getSample(cf_)
 
@@ -139,7 +139,7 @@ function (cfo::CalcFactor{<:MixtureFluxPose2Pose2})(meas1, meas2, Xi, Xj)
 
   # calculate the error for that measurement sample as Pose2Pose2
   #TODO, this is constructor in the hot loop is way to expensive.
-  cfZ = CalcFactor( cfo.factor.Z.mechanics, 0, length(cfo._legacyMeas), cfo._legacyMeas, cfo._legacyParams, cfo.cache, cfo.fullvariables, cfo.solvefor)
+  cfZ = CalcFactor( cfo.factor.Z.mechanics, 0, cfo._legacyParams, cfo.cache, cfo.fullvariables, cfo.solvefor, cfo.manifold)
 
   return  cfZ(meas1, Xi, Xj)
 

test/runtests.jl

@@ -19,6 +19,7 @@ testfiles = [
   # dev test first, for faster issues.
   # ...
   # "testFluxModelsPose2.jl";
+  "testPartialRangeCrossCorrelations.jl";
   "testG2oExportSE3.jl";
 
   #parametric tests
@@ -51,7 +52,6 @@ testfiles = [
   "testDidsonFunctions.jl";
   "testBasicPose2Stationary.jl";
   "TestPoseAndPoint2Constraints.jl";
-  "testPartialRangeCrossCorrelations.jl";
   "testDynPoint2D.jl";
   "testDeltaOdo.jl";
   "testFixedLagFG.jl";

test/testPartialPose3.jl

@@ -1,5 +1,6 @@
 # test partial pose3 constraints and evaluation
 
+# using Revise
 using Statistics
 using RoME
 using Test
@@ -28,8 +29,12 @@ f0 = addFactor!(fg, [:x1], PriorPose3(MvNormal([0.0, 5.0, 9.0, 0.1, 0.0, pi/2],
 prpz = PriorPose3ZRP( Normal(11.0, 1.0), MvNormal( [-0.1, 0.0], diagm([0.1, 0.1].^2) ))
 f1 = addFactor!(fg, [:x1], prpz)
 
-sf = sampleFactor(fg, :x1f2, 100)
-mu = getCoordinates(Pose3, mean(M, sf))
+sf = sampleFactor(fg, :x1f2, N)
+
+Mzrp = f1 |> getFactorType |> getManifold
+zrp0 = identity_element(Mzrp)
+mu = vee(Mzrp, zrp0, log(Mzrp, zrp0, mean(Mzrp, sf)))
+# mu = getCoordinates(Pose3, mean(Mzrp, sf)) # M # starting to improve partials, getManifold of partial factor returns whatever that manifold is
 
 solveTree!(fg)