Merge pull request #609 from JuliaRobotics/22Q3/perform/pose_br

StaticArray Pose2Pose2 and Pose2Point2BearingRange

Author: Affie

src/factors/BearingRange2D.jl

@@ -36,20 +36,30 @@ function IIF.getMeasurementParametric(s::Pose2Point2BearingRange{<:Normal, <:Nor
   return meas, iΣ
 end
 
-function (cfo::CalcFactor{<:Pose2Point2BearingRange})(measX, p, l)
-  #
-  M = getManifold(cfo.factor)
+function (cfo::CalcFactor{<:Pose2Point2BearingRange})(_measX::AbstractArray{MT}, _p::AbstractArray{PT}, _l::AbstractArray{LT}) where {MT,PT,LT}
+  T = promote_type(MT, PT, LT)
+  measX = convert(ArrayPartition{T, Tuple{SMatrix{2, 2, T, 4}, SVector{1, T}}}, _measX)
+  p = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _p)
+  l = convert(SVector{2, T}, _l)
+  r = cfo(measX, p, l)
+  return r
+end
 
+function (cfo::CalcFactor{<:Pose2Point2BearingRange})(
+                  measX::ArrayPartition{<:Real}, 
+                  p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, 
+                  l::SVector{2,T}) where T<:Real
+  #
   # wl = l
   # wTp = p
   # pl = pTw*wl
   pl  =  transpose(p.x[2]) * (l - p.x[1])
+  # δθ = mθ - plθ
+  # δr = mr - plr
+  δθ = Manifolds.sym_rem(measX.x[1][2] - atan(pl[2], pl[1]))
+  δr = measX.x[2][1]  - norm(pl)
 
-  mθ,mr = vee(M, Manifolds.Identity(M), measX) 
-  δθ = mθ - atan(pl[2], pl[1])
-  δr = mr - norm(pl)
-
-  return [δθ; δr]
+  return SA[δθ, δr]
 end
 
 # quick check

src/factors/Pose2D.jl

@@ -40,15 +40,38 @@ function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, pp
   (;manifold=M, ϵ0=getPointIdentity(M), Xc=zeros(3), q̂=getPointIdentity(M))
 end
 
+@inline function _vee(::SpecialEuclidean{2}, X::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real
+  return SVector{3,T}(X.x[1][1],X.x[1][2],X.x[2][2])
+end
+
+@inline function _compose(::SpecialEuclidean{2}, 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 T<:Real
+  return ArrayPartition(p.x[1] + p.x[2]*q.x[1], p.x[2]*q.x[2])
+end
+
 # Assumes X is a tangent vector
-function (cf::CalcFactor{<:Pose2Pose2})(X, p, q)
+function (cf::CalcFactor{<:Pose2Pose2})(_X::AbstractArray{MT}, _p::AbstractArray{PT}, _q::AbstractArray{LT})  where {MT,PT,LT}
+  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}
 
-    q̂ = allocate(q) 
     M = getManifold(Pose2)
-    ϵ0 = getPointIdentity(M)
-    exp!(M, q̂, ϵ0, X)
-    Manifolds.compose!(M, q̂, p, q̂)   
-    Xc = vee(M, q, log!(M, q̂, q, q̂))
+    ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I))
+
+    ϵX = exp(M, ϵ0, X)
+    # q̂ = Manifolds.compose(M, p, ϵX)    
+    q̂ = _compose(M, p, ϵX)
+    X_hat = log(M, q, q̂)
+    # Xc = vee(M, q, X_hat)
+    Xc = _vee(M, X_hat)
     return Xc
 end
 

test/testParametric.jl

@@ -153,7 +153,7 @@ end
 
 ##
 @testset "Test Parametric PriorPoint2 and Pose2Point2BearingRange" begin
-fg = LightDFG( solverParams=SolverParams(algorithms=[:default, :parametric]))
+fg = GraphsDFG( solverParams=SolverParams(algorithms=[:default, :parametric]))
 
 addVariable!(fg, :x1, Pose2)
 addVariable!(fg, :l1, Point2)