Merge pull request #1339 from JuliaRobotics/21Q3/fix/identity

Fix identity to identity_element for Manifolds 0.6

Author: dehann

Project.toml

@@ -43,7 +43,7 @@ TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53"
 UUIDs = "cf7118a7-6976-5b1a-9a39-7adc72f591a4"
 
 [compat]
-ApproxManifoldProducts = "0.4.12"
+ApproxManifoldProducts = "0.4.13"
 BSON = "0.2, 0.3"
 Combinatorics = "1.0"
 DataStructures = "0.16, 0.17, 0.18"
@@ -55,7 +55,7 @@ FunctionalStateMachine = "0.2.9"
 JLD2 = "0.3, 0.4"
 JSON2 = "0.3"
 KernelDensityEstimate = "0.5.6"
-Manifolds = "0.5"
+Manifolds = "0.6"
 ManifoldsBase = "0.11, 0.12"
 MetaGraphs = "0.6.4"
 NLSolversBase = "7.6"

examples/dev/factors_sandbox.jl

@@ -16,7 +16,7 @@ function grad_PointPoint_distance(M, m, p, q)
 end
 
 function PointPoint_distance(M, X, p, q, ::MeasurementOnTangent)
-    q̂ = compose(M, p, exp(M, identity(M, p), X))
+    q̂ = compose(M, p, exp(M, identity_element(M, p), X))
     return distance(M, q, q̂)
 end
 

src/Factors/GenericFunctions.jl

@@ -15,7 +15,7 @@ end
 
 #::MeasurementOnTangent
 function distanceTangent2Point(M::AbstractGroupManifold, X, p, q)
-    q̂ = compose(M, p, exp(M, identity(M, p), X)) #for groups
+    q̂ = compose(M, p, exp(M, identity_element(M, p), X)) #for groups
     # return log(M, q, q̂)
     return vee(M, q, log(M, q, q̂))
     # return distance(M, q, q̂)
@@ -53,7 +53,7 @@ end
 function default_identity(M::GroupManifold{ℝ, <:AbstractManifold})
     T = Float64
     s = representation_size(M)
-    return identity(M, zeros(SArray{Tuple{s...},T}))
+    return identity_element(M, zeros(SArray{Tuple{s...},T}))
 end
 
 # function default_identity(::SpecialOrthogonal{N}) where N
@@ -113,7 +113,7 @@ DFG.getManifold(f::ManifoldPrior) = f.M
 
 #TODO
 # function ManifoldPrior(M::AbstractGroupManifold, Z::SamplableBelief)
-#     # p = identity(M, #TOOD)
+#     # p = identity_element(M, #TOOD)
 #     # similar to getPointIdentity(M)
 #     return ManifoldPrior(M, Z, p)
 # end

src/Factors/Mixture.jl

@@ -87,7 +87,7 @@ function getSample( cf::CalcFactor{<:Mixture},
   for i in 1:N
     mixComponent = cf.factor.components[cf.factor.labels[i]]
     # measurements relate to the factor's manifold (either tangent vector or manifold point)
-    _setPointsMani!(smpls[1][i], randToPoints(mixComponent,1)[1])
+    setPointsMani!(smpls[1][i], randToPoints(mixComponent,1)[1])
   end
 
   # TODO only does first element of meas::Tuple at this stage, see #1099

src/NumericalCalculations.jl

@@ -85,7 +85,7 @@ function _solveLambdaNumeric( fcttype::Union{F,<:Mixture{N_,F,S,T}},
   # the variable is a manifold point, we are working on the tangent plane in optim for now.
   # 
   #TODO this is not general to all manifolds, should work for lie groups.
-  # ϵ = identity(M, u0)
+  # ϵ = identity_element(M, u0)
   ϵ = getPointIdentity(variableType)
   # X0c = get_coordinates(M, u0, log(M, ϵ, u0), DefaultOrthogonalBasis()) 
   X0c = vee(M, u0, log(M, ϵ, u0)) 

src/SolverUtilities.jl

@@ -22,55 +22,6 @@ randToPoints(distr::SamplableBelief, N::Int=1) = [rand(distr,1)[:] for i in 1:N]
 randToPoints(distr::ManifoldKernelDensity, N::Int=1) = rand(distr,N)
 
 
-_setPointsMani!(dest::AbstractVector, src::AbstractVector) = (dest .= src)
-_setPointsMani!(dest::AbstractMatrix, src::AbstractMatrix) = (dest .= src)
-function _setPointsMani!(dest::AbstractVector, src::AbstractMatrix)
-  @assert size(src,2) == 1 "Workaround _setPointsMani! currently only allows size(::Matrix, 2) == 1"
-  _setPointsMani!(dest, src[:])
-end
-function _setPointsMani!(dest::AbstractMatrix, src::AbstractVector)
-  @assert size(dest,2) == 1 "Workaround _setPointsMani! currently only allows size(::Matrix, 2) == 1"
-  _setPointsMani!(view(dest,:,1), src)
-end
-
-function _setPointsMani!(dest::AbstractVector, src::AbstractVector{<:AbstractVector})
-  @assert length(src) == 1 "Workaround _setPointsMani! currently only allows Vector{Vector{P}}(...) |> length == 1"
-  _setPointsMani!(dest, src[1])
-end
-
-function _setPointsMani!(dest::ProductRepr, src::ProductRepr)
-  for (k,prt) in enumerate(dest.parts)
-    _setPointsMani!(prt, src.parts[k])
-  end
-end
-
-# asPartial=true indicates that src coords are smaller than dest coords, and false implying src has dummy values in placeholder dimensions
-function _setPointsManiPartial!(Mdest::AbstractManifold, 
-                                dest, 
-                                Msrc::AbstractManifold, 
-                                src, 
-                                partial::AbstractVector{<:Integer},
-                                asPartial::Bool=true )
-  #
-
-  e0 = identity(Mdest, dest)
-  dest_ = vee(Mdest, e0, log(Mdest, e0, dest))
-
-  e0s = identity(Msrc, src)
-  src_ = vee(Msrc, e0s, log(Msrc, e0s, src))
-
-  # do the copy in coords 
-  dest_[partial] .= asPartial ? src_ : view(src_, partial)
-
-  # update points base in original
-  dest__ = exp(Mdest, e0, hat(Mdest, e0, dest_))
-  _setPointsMani!(dest, dest__)
-
-  #
-  return dest 
-end
-
-
 
 """
     $TYPEDSIGNATURES
@@ -79,10 +30,11 @@ Calculate the Kernel Embedding MMD 'distance' between sample points (or kernel d
 
 Notes
 - `bw::Vector=[0.001;]` controls the mmd kernel bandwidths.
+- Overloading from ApproxManifoldProducts
 
 Related
 
-`KDE.kld`
+`AMP.kld`
 """
 function mmd( p1::AbstractVector{P1}, 
               p2::AbstractVector{P2}, 

src/Variables/DefaultVariables.jl

@@ -43,7 +43,7 @@ DFG.getDimension(val::InstanceType{ContinuousEuclid{N}}) where N = manifold_dime
 # DFG.getDimension(val::ContinuousEuclid{N}) where N = manifold_dimension(getManifold(val))
 
 DFG.getPointType(::Type{ContinuousEuclid{N}}) where N = Vector{Float64}
-DFG.getPointIdentity(M_::Type{ContinuousEuclid{N}}) where N = zeros(N) # identity(getManifold(M_), zeros(N)) 
+DFG.getPointIdentity(M_::Type{ContinuousEuclid{N}}) where N = zeros(N) # identity_element(getManifold(M_), zeros(N)) 
 
 
 Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::InstanceType{ContinuousEuclid{N}}) where N = TranslationGroup(N)

src/services/EvalFactor.jl

@@ -364,7 +364,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
     for m in (1:length(addEntr))[ahmask]
       # FIXME, selection for all measurement::Tuple elements
       # @info "check broadcast" ccwl.usrfnc! addEntr[m] ccwl.measurement[1][m]
-      _setPointsMani!(addEntr[m], ccwl.measurement[1][m])
+      setPointsMani!(addEntr[m], ccwl.measurement[1][m])
     end
     # ongoing part of RoME.jl #244
     addEntropyOnManifold!(mani, addEntrNH, 1:getDimension(mani), spreadDist)
@@ -380,7 +380,7 @@ function evalPotentialSpecific( Xi::AbstractVector{<:DFGVariable},
         # FIXME, need ability to replace partial points
         partialCoords = ccwl.partialDims
         Msrc, = getManifoldPartial(mani,partialCoords)
-        _setPointsManiPartial!(mani, addEntr[m], Msrc, ccwl.measurement[1][m], partialCoords)
+        setPointsManiPartial!(mani, addEntr[m], Msrc, ccwl.measurement[1][m], partialCoords)
         # addEntr[m][dimnum] = ccwl.measurement[1][m][i]
       # end
     end

src/services/FactorGradients.jl

@@ -108,7 +108,7 @@ end
 getCoordSizes(fgc::FactorGradientsCached!) = fgc._coord_sizes
 
 
-_setFGCSlack!(fgc::FactorGradientsCached!{F}, slack) where F = _setPointsMani!(fgc.slack_residual, slack)
+_setFGCSlack!(fgc::FactorGradientsCached!{F}, slack) where F = setPointsMani!(fgc.slack_residual, slack)
 
 function _setFGCSlack!(fgc::FactorGradientsCached!{F,S}, slack::Number) where {F,S<:Number}
   fgc.slack_residual = slack
@@ -121,7 +121,7 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
 
   # update in-place the new measurement value in preparation for new gradient calculation
   for (m, tup_m) in enumerate(fgc.measurement)
-    _setPointsMani!(tup_m, meas_pts[m])
+    setPointsMani!(tup_m, meas_pts[m])
   end
 
   # update the residual _slack in preparation for new gradient calculation
@@ -132,12 +132,12 @@ function (fgc::FactorGradientsCached!)(meas_pts...)
   new_slack = calcFactorResidualTemporary(fct, varTypes, measurement, pts; tfg=fgc._tfg)
   # TODO make sure slack_residual is properly wired up with all the lambda functions as expected
   _setFGCSlack!(fgc, new_slack)
-  # _setPointsMani!(fgc.slack_residual, new_slack)
+  # setPointsMani!(fgc.slack_residual, new_slack)
 
   # set new points in preparation for new gradient calculation
   for (s,pt) in enumerate(meas_pts[(lenm+1):end])
     # update the local memory in fgc to take the values of incoming `pts`
-    _setPointsMani!(fgc.currentPoints[s], pt)
+    setPointsMani!(fgc.currentPoints[s], pt)
   end
 
   # update the gradients at new values contained in fgc

test/testSpecialEuclidean2Mani.jl

@@ -165,9 +165,9 @@ DFG.getManifold(::ManiPose2Point2) = TranslationGroup(2)
 function (cfo::CalcFactor{<:ManiPose2Point2})(measX, p, q)
   #
     M = SpecialEuclidean(2)
-    q_SE = ProductRepr(q, identity(SpecialOrthogonal(2), p.parts[2]))
+    q_SE = ProductRepr(q, identity_element(SpecialOrthogonal(2), p.parts[2]))
 
-    X_se2 = log(M, identity(M, p), compose(M, inv(M, p), q_SE))
+    X_se2 = log(M, identity_element(M, p), compose(M, inv(M, p), q_SE))
     X = X_se2.parts[1]
     # NOTE wrong for what we want X̂ = log(M, p, q_SE)
     return measX - X