Merge pull request #611 from JuliaRobotics/22Q3/feat/g2o_parsers

Parse more G2O formats and Improve PriorPose2 performance

Author: Affie

src/factors/Pose2D.jl

@@ -40,14 +40,6 @@ 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::AbstractArray{MT}, _p::AbstractArray{PT}, _q::AbstractArray{LT})  where {MT,PT,LT}
   T = promote_type(MT, PT, LT)

src/factors/PriorPose2.jl

@@ -16,29 +16,32 @@ end
 
 DFG.getManifold(::InstanceType{PriorPose2}) = getManifold(Pose2) # SpecialEuclidean(2)
 
-function (cf::CalcFactor{<:PriorPose2})(m, p)
-  M = getManifold(Pose2)
-  Xc = vee(M, p, log(M, p, m))
-  return Xc
+@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
 
-  # M = getManifold(Pose2)
-  # # X = allocate(p)
-  # # X = ProductRepr(zeros(MVector{2}), zeros(MMatrix{2,2}))
-  # # log!(M, X, p, m)
-  # X = log(M, p, m)
-  # return  SA[X.x[1][1],X.x[1][2],X.x[2][2]]
+@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
 
+function (cf::CalcFactor{<:PriorPose2})(_m::AbstractArray{MT}, _p::AbstractArray{PT})  where {MT<:Real,PT<:Real}
+  T = promote_type(MT, PT)
+  m = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _m)
+  p = convert(ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}, _p)
+  return cf(m,p)
 end
-# BenchmarkTools.Trial: 10000 samples with 1 evaluation.
-#  Range (min … max):  22.299 μs …  11.920 ms  ┊ GC (min … max): 0.00% … 98.28%
-#  Time  (median):     29.930 μs               ┊ GC (median):    0.00%
-#  Time  (mean ± σ):   40.097 μs ± 171.479 μs  ┊ GC (mean ± σ):  5.60% ±  1.39%
 
-#    ▂▆█▇▆▅▄▃▃▄▃▂        ▂▂▂▂▁                                   ▂
-#   ▆██████████████▆▆▆▄▅▇██████▇▇█▆▇▆▄▅▄▆▆▅▅▅▆▅▅▅▄▅▄▄▄▅▅▅▄▄▄▃▅▄▄ █
-#   22.3 μs       Histogram: log(frequency) by time       127 μs <
+# TODO the log here looks wrong (for gradients), consider:
+# X = log(p⁻¹ ∘ m) 
+# X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m))
+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
 
-#  Memory estimate: 26.88 KiB, allocs estimate: 481.
+  M = getManifold(Pose2)
+  Xc = _vee(M, log(M, p, m))
+  return Xc
+end
 
 #TODO Serialization of reference point p 
 ## Serialization support

src/g2oParser.jl

@@ -53,8 +53,24 @@ end
 Parses a single g2o instruction to add information to factor graph.
 """
 function parseG2oInstruction!(fg::AbstractDFG,
-                              instruction::Array{SubString{String},1})
-    if instruction[1] == "EDGE_SE2"
+                              instruction::Array{SubString{String},1};
+                              initialize::Bool=true)
+    #
+    infoM6 = zeros(6,6)
+
+    if instruction[1] == "VERTEX_SE2"
+          poseId = Symbol("x", instruction[2])
+          x = parse(Float64,instruction[3])
+          y = parse(Float64,instruction[4])
+          θ = parse(Float64,instruction[5])
+          #NOTE just useing some covariance as its not included
+          cov = diagm([1,1,0.1])
+          addVariable!(fg, poseId, Pose2)
+          if initialize
+              # initVariable!(fg, poseId, MvNormal([x,y,θ],cov))
+              initVariable!(fg, poseId, MvNormal([x,y,θ],cov), :parametric)
+          end
+    elseif instruction[1] == "EDGE_SE2"
         # Need to add a relative pose measurement between two variables.
         # Parse all of the variables starting with the symbols.
         from_pose = Symbol("x", instruction[2])
@@ -86,6 +102,52 @@ function parseG2oInstruction!(fg::AbstractDFG,
         measurement = MvNormal([x_state; y_state; theta_state], cov_mat)
         rel_pose_factor = Pose2Pose2(measurement)
         addFactor!(fg, [from_pose, to_pose], rel_pose_factor)
+    elseif instruction[1] == "EDGE_SE3:QUAT"
+
+        # MU1 = Unitary(1,ℍ)
+        # ϵU1 = identity_element(MU1)
+        MSO3 = SpecialOrthogonal(3)
+        ϵSO3 = identity_element(MSO3)
+
+         # Need to add a relative pose measurement between two variables.
+        # Parse all of the variables starting with the symbols.
+        from_pose = Symbol("x", instruction[2])
+        to_pose = Symbol("x", instruction[3])
+
+        dt = parse.(Float64, instruction[4:6])
+        qvec = parse.(Float64, instruction[[10;7:9]])
+        
+        dR = TU.convert(SO3, TU.Quaternion(qvec[1],qvec[2:4])).R 
+        X = log(MSO3, ϵSO3, dR) 
+        Xc = vee(MSO3, ϵSO3, X)
+
+        a = parse.(Float64, instruction[11:31])
+        for i=1:6
+            vw = view(a, (1+(i-1)*(14-i)÷2):(i*(13-i)÷2))
+            infoM6[i,i:6] = vw 
+            infoM6[i:6,i] = vw
+        end
+        cov_mat = inv(infoM6)
+        # NOTE workaround to ensure cov_mat is Hermitian -- TODO use inf_mat directly for MvNormal
+        cov_mat += cov_mat'
+        cov_mat ./= 2
+        
+        # Make sure both variables are in the FG. Otherwise add them.
+        if (from_pose in ls(fg)) == false
+            addVariable!(fg, from_pose, Pose3)
+        end
+        if (to_pose in ls(fg)) == false
+            addVariable!(fg, to_pose, Pose3)
+        end
+
+        #FIXME conversion between U(1,ℍ) and SO3 covariances?
+        @error "3D covariance from Quaternion is not done yet!" maxlog=1
+        # dq = Manifolds.Quaternion(qvec...)
+
+        # Add a factor between the two poses with the relative measurement.
+        measurement = MvNormal([dt;Xc], cov_mat)
+        rel_pose_factor = Pose3Pose3(measurement)
+        addFactor!(fg, [from_pose, to_pose], rel_pose_factor)
     end
     return fg
 end