From 5f35742e4df2e4e52e5607d579c7a32f49f3d1a4 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 17 Aug 2022 12:49:54 +0200 Subject: [PATCH 1/3] Parse more G2O formats --- src/g2oParser.jl | 66 ++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 64 insertions(+), 2 deletions(-) diff --git a/src/g2oParser.jl b/src/g2oParser.jl index 4a74c763..4cfb6b5e 100644 --- a/src/g2oParser.jl +++ b/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 From ad8fb6787042713f7209aadcb9f545365d103642 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 17 Aug 2022 12:50:29 +0200 Subject: [PATCH 2/3] Improve PriorPose2 performance --- src/factors/Pose2D.jl | 8 -------- src/factors/PriorPose2.jl | 39 +++++++++++++++++++++------------------ 2 files changed, 21 insertions(+), 26 deletions(-) diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 4a81c2c2..6ea3910f 100644 --- a/src/factors/Pose2D.jl +++ b/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) diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index 81792e7e..dde2a66d 100644 --- a/src/factors/PriorPose2.jl +++ b/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 From 6979b9e73aadaeb56f0e95c35adb907cc138bfe3 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Wed, 17 Aug 2022 17:14:09 +0200 Subject: [PATCH 3/3] fix unicode char - --- src/g2oParser.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/g2oParser.jl b/src/g2oParser.jl index 4cfb6b5e..c1fe59a5 100644 --- a/src/g2oParser.jl +++ b/src/g2oParser.jl @@ -123,7 +123,7 @@ function parseG2oInstruction!(fg::AbstractDFG, a = parse.(Float64, instruction[11:31]) for i=1:6 - vw = view(a, (1+(i−1)*(14−i)÷2):(i*(13-i)÷2)) + vw = view(a, (1+(i-1)*(14-i)÷2):(i*(13-i)÷2)) infoM6[i,i:6] = vw infoM6[i:6,i] = vw end