From 44653e0f3dbe48a4927bb70ecb3b17c0a97b1caa Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 23 May 2025 11:56:35 -0400 Subject: [PATCH 01/18] Update Pose2 to LieGroups --- .gitignore | 2 +- Project.toml | 2 ++ src/RoME.jl | 5 +-- src/factors/Inertial/IMUDeltaFactor.jl | 2 +- src/factors/Pose2D.jl | 47 ++++++-------------------- src/variables/VariableTypes.jl | 2 +- 6 files changed, 18 insertions(+), 42 deletions(-) diff --git a/.gitignore b/.gitignore index 0f5ecf28..2f284fcc 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ -/dev +dev Manifest.toml *.so *.cov diff --git a/Project.toml b/Project.toml index 27b48231..a4ee4478 100644 --- a/Project.toml +++ b/Project.toml @@ -18,6 +18,7 @@ ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534" IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480" Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" +LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" @@ -65,6 +66,7 @@ ImageIO = "0.6" IncrementalInference = "0.35" Interpolations = "0.14, 0.15" KernelDensityEstimate = "0.5.1, 0.6" +LieGroups = "0.1.1" LinearAlgebra = "1.10" Manifolds = "0.10.1" ManifoldsBase = "0.15, 1" diff --git a/src/RoME.jl b/src/RoME.jl index 1ae4c77c..85190ee8 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -21,7 +21,8 @@ using DistributedFactorGraphs, TensorCast, ManifoldsBase, - Manifolds + Manifolds, + LieGroups using StaticArrays using PrecompileTools @@ -33,7 +34,7 @@ using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, SpecialOr import Manifolds: project, project!, identity_element import Rotations as _Rot -import Rotations: ⊕, ⊖ # TODO deprecate +# import Rotations: ⊕, ⊖ # TODO deprecate export SpecialOrthogonal, SpecialEuclidean export submanifold_component diff --git a/src/factors/Inertial/IMUDeltaFactor.jl b/src/factors/Inertial/IMUDeltaFactor.jl index d4cde032..4cc61798 100644 --- a/src/factors/Inertial/IMUDeltaFactor.jl +++ b/src/factors/Inertial/IMUDeltaFactor.jl @@ -394,7 +394,7 @@ function (cf::CalcFactor{<:IMUDeltaFactor})( q_SE3, q_vel, b = zeros(SVector{6,Float64}) -) where T <: Real +) p = ArrayPartition(p_SE3.x[2], p_vel, p_SE3.x[1]) q = ArrayPartition(q_SE3.x[2], q_vel, q_SE3.x[1]) return cf(Δmeas, p, q, b) diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 7d2ee840..9f1f9b64 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -8,16 +8,15 @@ Rigid transform between two Pose2's, assuming (x,y,theta). Calcuated as: ```math \\begin{aligned} -\\hat{q}=\\exp_pX_m\\\\ -X = \\log_q \\hat{q}\\\\ -X^i = \\mathrm{vee}(q, X) +\\hat{X}_p=\\log_pq\\\\ +X^i = \\mathrm{vee}(X_m-\\hat{X}) \\end{aligned} ``` with: ``\\mathcal M= \\mathrm{SE}(2)`` Special Euclidean group\\ ``p`` and ``q`` ``\\in \\mathcal M`` the two Pose2 points\\ the measurement vector ``X_m \\in T_p \\mathcal M``\\ -and the error vector ``X \\in T_q \\mathcal M``\\ +and the error vector ``\\hat{X} \\in T_p \\mathcal M``\\ ``X^i`` coordinates of ``X`` DevNotes @@ -27,7 +26,7 @@ Related [`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref) """ -Base.@kwdef struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize +Base.@kwdef struct Pose2Pose2{T<:IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0])) end @@ -35,45 +34,19 @@ DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2; vect Pose2Pose2(::UniformScaling) = Pose2Pose2() - -# Assumes X is a tangent vector -function (cf::CalcFactor{<:Pose2Pose2})(_X::AbstractArray{MT}, _p::AbstractArray{PT}, _q::AbstractArray{LT}) where {MT,PT,LT} - #TODO remove this convertions - # @warn "This warning should not be triggered after StaticArrays upgrade" maxlog=10 - 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} - - M = getManifold(Pose2) - # ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I)) - ϵ0 = getPointIdentity(M) - - ϵX = exp(M, ϵ0, X) - # q̂ = Manifolds.compose(M, p, ϵX) - q̂ = _compose(M, p, ϵX) - X_hat = log(M, q, q̂)#::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}} - # Xc = vee(M, q, X_hat) - Xc = _vee(M, X_hat)#::SVector{3,T} - return Xc +function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) + G = getManifold(Pose2) + X̂ = log(base_manifold(G), getPointIdentity(G), compose(G, inv(G, p), q)) + return vee(LieAlgebra(G), X - X̂) end - # NOTE, serialization support -- will be reduced to macro in future # ------------------------------------ """ $(TYPEDEF) """ -Base.@kwdef struct PackedPose2Pose2 <: AbstractPackedFactor +Base.@kwdef struct PackedPose2Pose2 <: AbstractPackedFactor Z::PackedSamplableBelief end function convert(::Type{Pose2Pose2}, d::PackedPose2Pose2) @@ -85,7 +58,7 @@ end # FIXME, rather have separate compareDensity functions -function compare(a::Pose2Pose2,b::Pose2Pose2; tol::Float64=1e-10) +function compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64=1e-10) return compareDensity(a.Z, b.Z) end diff --git a/src/variables/VariableTypes.jl b/src/variables/VariableTypes.jl index 84b02d60..a9ec7acc 100644 --- a/src/variables/VariableTypes.jl +++ b/src/variables/VariableTypes.jl @@ -32,7 +32,7 @@ $(TYPEDEF) Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM. """ -@defVariable Pose2 SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +@defVariable Pose2 SpecialEuclideanGroup(2; variant=:right) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) """ $(TYPEDEF) From c97a64f3cb4e17612a900e43e02ff2247f65c34f Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 12 Jun 2025 11:50:06 +0200 Subject: [PATCH 02/18] Prior Pose2 --- src/factors/Pose2D.jl | 2 +- src/factors/PriorPose2.jl | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 9f1f9b64..a0ea88a4 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -36,7 +36,7 @@ Pose2Pose2(::UniformScaling) = Pose2Pose2() function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) G = getManifold(Pose2) - X̂ = log(base_manifold(G), getPointIdentity(G), compose(G, inv(G, p), q)) + X̂ = log(base_manifold(G), getPointIdentity(G), LieGroups.compose(G, inv(G, p), q)) return vee(LieAlgebra(G), X - X̂) end diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index 1bd1c213..340c2574 100644 --- a/src/factors/PriorPose2.jl +++ b/src/factors/PriorPose2.jl @@ -39,11 +39,11 @@ function (cf::CalcFactor{<:PriorPose2})( p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real M = getManifold(Pose2) - ϵ = getPointIdentity(M) - Xc = _vee(M, log(M, p, m)) - # X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m)) - # Xc = vee(M, ϵ, X) - return Xc + ϵ = getPointIdentity(Pose2) + # Xc = _vee(M, log(M, p, m)) + # X = log(M, p, m) + X = log(base_manifold(M), ϵ, LieGroups.compose(M, inv(M, p), m)) + return vee(LieAlgebra(M), X) end #TODO Serialization of reference point p From 5ee1097eaa938d326340cc15ff537a759854135e Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 24 Jun 2025 13:41:14 +0200 Subject: [PATCH 03/18] TR vs exp sandbox --- test/testSEParameterizations.jl | 199 ++++++++++++++++++++++++++++++++ 1 file changed, 199 insertions(+) create mode 100644 test/testSEParameterizations.jl diff --git a/test/testSEParameterizations.jl b/test/testSEParameterizations.jl new file mode 100644 index 00000000..aa2373b0 --- /dev/null +++ b/test/testSEParameterizations.jl @@ -0,0 +1,199 @@ +using LieGroups +using DistributedFactorGraphs +using Distributions +using StaticArrays +using DistributedFactorGraphs: ArrayPartition + +T = ArrayPartition{Float64, Tuple{Matrix{Float64}, Vector{Float64}}} +G = SpecialEuclideanGroup(2; variant=:right) +ε = identity_element(G, T) + +# T-R Coordinates, Chirikjian, p.35 Jr, above 10.76 +function Jr(::Val{:SE2_TR}, Xⁱ::AbstractVector) + @assert length(Xⁱ) == 3 "Xⁱ must be a vector of length 3" + # Xⁱ = [x1, x2, θ] + θ = Xⁱ[3] + return [ + cos(θ) sin(θ) 0; + -sin(θ) cos(θ) 0; + 0 0 1 + ] +end + +# Exponential Coordinates, Chirikjian, p.36 Jr, after 10.76 +function Jr(::Val{:SE2_EXP}, Xⁱ::AbstractVector) + @assert length(Xⁱ) == 3 "Xⁱ must be a vector of length 3" + v1 = Xⁱ[1] + v2 = Xⁱ[2] + α = Xⁱ[3] + if α == 0 + return [1 0 0; + 0 1 0; + 0 0 1.] + end + return [ + (sin(α))/α (1 - cos(α))/α (α*v1 - v2 + v2*cos(α) - v1*sin(α))/α^2; + (cos(α) - 1)/α sin(α)/α (v1 + α*v2 - v1*cos(α) - v2*sin(α))/α^2; + 0 0 1 + ] +end + +function Ad(::typeof(SpecialEuclideanGroup(2; variant=:right)), p) + t = p.x[1] + R = p.x[2] + vcat( + hcat(R, -SA[0 -1; 1 0]*t), + SA[0 0 1] + ) +end + +Ad(G, p) + + +## +# T-R Coordinates +X₀aⁱ = [10, 1, pi/4] +# LieAlgebra(G) is a Fiber? +X₀a = hat(LieAlgebra(G), X₀aⁱ, T) +# base_manifold(G) -> use the default Riemannian metric +p = exp(base_manifold(G), ε, X₀a) +# Exponential Coordinates +X₀b = log(G, p) + +# J = XbJp * pJXa +# J = jacobian_of_log(SE2, p)_wrt_p * jacobain_of_exp(SO2xEuclid2, Xa)_wrt_Xa +# J = J(Log(p), p) * J(exp(Xa), Xa) +J = inv(Jr(Val(:SE2_EXP), vee(LieAlgebra(G), X₀b))) * Jr(Val(:SE2_TR), vee(LieAlgebra(G), X₀a)) + +Σα = [ + 1 0 0; + 0 0.1 0; + 0 0 0.01 +] + +N = 500000 +# generate noise as tangent T-R Coordinates (assuming observation was in T-R) +Xⁱs = rand(MvNormal(Σα), N) + +# map from T-R to exponential tangent representation +function φ(X) # work on better name + # use Riemannian Exponential map to get the points (Chirikjian, p34, (eq. 10.75)) + np = LieGroups.compose(G, p, exp(base_manifold(G), ε, X)) # noisy p + # convert the points back using the group logarithm map + return log(G, p, np) # -> Y +end + +# Convert noisy coordinates in T-R coordinates to Exponential Coordinates at expansion point p +# is this a push-forward? disagreeement from Mateusz/Ronnie... +Yⁱs = map(eachcol(Xⁱs)) do Xⁱ + # vector in a tangent space represented from T-R coordinates, [hybrid tangent representation] + X = hat(LieAlgebra(G), Xⁱ, T) + # map from T-R to exponential tangent representation + Y = φ(X) + # get coordinates in exponential representation - Chirikjian, p35, (eq. 10.76) + Yⁱ = vee(LieAlgebra(G), Y) + return Yⁱ +end + + +Yⁱs = hcat(Yⁱs...) + +fit_Σβ = cov(fit_mle(MvNormal, Yⁱs)) + +# is fit_Σβ now the covariance converted from Σα in T-R coordinates to exponential coordinates at point p on G? + +# is this a valid test? + +# J = XbJp * pJXa +# J = jacobian_of_log(SE2, p)_wrt_p * jacobain_of_exp(SO2xEuclid2, Xa)_wrt_Xa +# J = J(Log(p), p) * J(exp(Xa), Xa) +J = inv(Jr(Val(:SE2_EXP), vee(LieAlgebra(G), Y₀))) * Jr(Val(:SE2_TR), vee(LieAlgebra(G), X₀)); + +# Sola 2018, eq. 55 +prop_Σβ = J * Σα * J' +fit_Σβ +# why do this? comparing what with what? +# What: linear propagate vs nonparametric mapping test of Covariance matrix +# Why: Trying resolve how to do robotics rigid transforms with new LieGroups.jl +isapprox(prop_Σβ, fit_Σβ; atol=1e-3) + + +## ================================================================================= +## +## ================================================================================= + +# set Chirikjian 10.75 = 10.76 +# x1 = (v2*(-1 + cos(α)) + v1*sin(α))/α +# x2 = (v1*( 1 - cos(α)) + v2*sin(α))/α +# θ = α + +function map_TR_to_exp_coords(g) + x1 = g[1] + x2 = g[2] + θ = g[3] + + α = θ + + #FIXME is this correct? + if α == 0 + return [x1, x2, θ] + end + + #FIXME maybe simplify this equations further + v2 = (x1*α - x1*cos(α)*α - x2*sin(α)*α) / (2*(cos(α) - 1)) + v1 = (x2*α - v2*sin(α)) / (1 - cos(α)) + + return [v1, v2, α] +end + +function map_exp_to_TR_coords(g) + v1 = g[1] + v2 = g[2] + α = g[3] + + θ = α + + x1 = (v2*(-1 + cos(α)) + v1*sin(α))/α + x2 = (v1*( 1 - cos(α)) + v2*sin(α))/α + + return [x1, x2, θ] +end + +X₀ⁱ = [10, 1, pi/2] +X₀ = hat(LieAlgebra(G), X₀ⁱ, T) +p = exp(base_manifold(G), ε, X₀) +Y₀ = log(G, p) +Y₀ⁱ = vee(LieAlgebra(G), Y₀) + +Yⁱ = map_TR_to_exp_coords(X₀ⁱ) +isapprox(Y₀ⁱ, Yⁱ) + +Xⁱ = map_exp_to_TR_coords(Yⁱ) +isapprox(X₀ⁱ, Xⁱ) + +N = 50000 +# generate noise as tangent T-R Coordinates in the lie algebra +Xⁱs = rand(MvNormal(Σα), N) +Yⁱs = map(eachcol(Xⁱs)) do Xⁱ + Yⁱ = map_TR_to_exp_coords(Xⁱ) + return Yⁱ +end + +Yⁱs = hcat(Yⁱs...) + +fit_Σβ = cov(fit_mle(MvNormal, Yⁱs)) + +Jfd = FiniteDiff.finite_difference_jacobian(map_TR_to_exp_coords, [0.,0,0]) +prop_Σβ = Jfd * Σα * Jfd' +fit_Σβ + + X = hat(LieAlgebra(G), Xⁱ, T) + q = exp(G, p, X) + vee(LieAlgebra(G), log(G, p, q)) +end + +jac_Xbs = hcat(jac_Xbs...) + + +scatter(Xbs[1:2, :]; axis=(aspect=DataAspect(),), markersize=3) +scatter!(jac_Xbs[1:2, :]; markersize=3) \ No newline at end of file From 47508ad82aebf86c3ed648900f527983e13e5a7c Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 26 Jun 2025 12:57:14 +0200 Subject: [PATCH 04/18] Experiment with MetricManifold --- test/testSOnxRnManifold.jl | 114 +++++++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) create mode 100644 test/testSOnxRnManifold.jl diff --git a/test/testSOnxRnManifold.jl b/test/testSOnxRnManifold.jl new file mode 100644 index 00000000..07771985 --- /dev/null +++ b/test/testSOnxRnManifold.jl @@ -0,0 +1,114 @@ + + +# Rigid Body Kinematics Metric CrokeKumar eq 61. +# A family of left invariant metrics: +# G = [αI 0; 0 βI] +# where α and β are arbitrary constants, satisfies all the equations (80). +# This are the only left-invariant metrics which are compatible with the acceleration connection. +# Can we add α and β as parameters to the metric? +struct LeftInvariantRBKMetric <: RiemannianMetric end + +SOnxRnManifold(n) = MetricManifold(SpecialEuclideanGroup(n; variant=:right), LeftInvariantRBKMetric()) + +SOnxRnManifoldType = Union{typeof(SOnxRnManifold(2)), typeof(SOnxRnManifold(3))} + +Manifolds.identity_element(::typeof(SOnxRnManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +Manifolds.identity_element(::typeof(SOnxRnManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) + +# geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3 +function Manifolds.exp(M::SOnxRnManifoldType, p, X) + G = base_manifold(M) + ε = identity_element(M) + return compose(G, p, exp(base_manifold(G), ε, X)) +end + +function Manifolds.log(M::SOnxRnManifoldType, p, q) + G = base_manifold(M) + ε = identity_element(M) + X = log(base_manifold(G), ε, compose(G, inv(G, p), q)) + return X +end + +function Manifolds.inner(M::SOnxRnManifoldType, p, X, Y) + Xtr = submanifold_components(M, X)[1] + XRo = submanifold_components(M, X)[2] + Ytr = submanifold_components(M, Y)[1] + YRo = submanifold_components(M, Y)[2] + # Metric on Chirikjian, p35 W = diagm([1,1,2]) + return dot(Xtr,Ytr) + dot(XRo, YRo)/2 +end + + + + + + + + +## ======================================================================================= +## SE2 + metric + +M = SOnxRnManifold(2) +G = base_manifold(M) +ε = identity_element(M) +T = typeof(identity_element(M)) +Xⁱ = [10, 1, pi/4] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) +q = compose(G, p, exp(base_manifold(G), ε, X)) + +q ≈ exp(M, p, X) +X ≈ log(M, p, q) + + +Xⁱ = [1, 1, 1] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) + +W = diagm([1,1,2]) + +XX = hat(LieAlgebra(G), Xⁱ) +0.5*tr(XX*W*XX') +inner(M, p, X, X) +inner(base_manifold(G), p, X, X) + +Manifolds.distance(M, ε, p) +Manifolds.distance(base_manifold(G), ε, p) + +## SE3 + metric + +M = SOnxRnManifold(3) +G = base_manifold(M) +ε = identity_element(M) +T = typeof(identity_element(M)) +Xⁱ = [10, 1, 0, 0, 0, pi/4] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) +q = compose(G, p, exp(base_manifold(G), ε, X)) + +q ≈ exp(M, p, X) +X ≈ log(M, p, q) + + +Xⁱ = [0, 0, 0, 0, 1, 1] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) + +0.5*tr(X.x[2]*X.x[2]') +dot(X.x[2], X.x[2]) + +inner(M, p, X, X) +inner(base_manifold(G), p, X, X) + +Manifolds.distance(M, ε, p) +Manifolds.distance(base_manifold(G), ε, p) + +Xⁱ = [1, 1, 0, 0, 1, 1] +W = diagm([1,1,1,2]) +X = hat(LieAlgebra(G), Xⁱ, T) +XX = hat(LieAlgebra(G), Xⁱ) +0.5*tr(XX*W*XX') +inner(M, p, X, X) +inner(base_manifold(G), p, X, X) + + From fb2328f8c4e4eed2fc54b0b32471e0a025526725 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Thu, 26 Jun 2025 16:52:34 +0200 Subject: [PATCH 05/18] use typeof(p) in identity_element --- test/testSOnxRnManifold.jl | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/test/testSOnxRnManifold.jl b/test/testSOnxRnManifold.jl index 07771985..c29323fd 100644 --- a/test/testSOnxRnManifold.jl +++ b/test/testSOnxRnManifold.jl @@ -12,19 +12,16 @@ SOnxRnManifold(n) = MetricManifold(SpecialEuclideanGroup(n; variant=:right), Lef SOnxRnManifoldType = Union{typeof(SOnxRnManifold(2)), typeof(SOnxRnManifold(3))} -Manifolds.identity_element(::typeof(SOnxRnManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) -Manifolds.identity_element(::typeof(SOnxRnManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) - # geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3 function Manifolds.exp(M::SOnxRnManifoldType, p, X) G = base_manifold(M) - ε = identity_element(M) + ε = identity_element(M, typeof(p)) return compose(G, p, exp(base_manifold(G), ε, X)) end function Manifolds.log(M::SOnxRnManifoldType, p, q) G = base_manifold(M) - ε = identity_element(M) + ε = identity_element(M, typeof(p)) X = log(base_manifold(G), ε, compose(G, inv(G, p), q)) return X end From 20a89924403a2c42eaffcfac770959fdd2a7678e Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 1 Jul 2025 11:20:06 +0200 Subject: [PATCH 06/18] Use LeftInvariantKinematicMetric for Pose2 --- Project.toml | 4 +- src/RoME.jl | 2 + src/factors/Pose2D.jl | 7 +- src/factors/PriorPose2.jl | 18 +-- src/manifolds/SOnxRn_MetricManifold.jl | 161 +++++++++++++++++++++++++ src/variables/VariableTypes.jl | 2 +- test/testSOnxRnManifold.jl | 111 ----------------- 7 files changed, 172 insertions(+), 133 deletions(-) create mode 100644 src/manifolds/SOnxRn_MetricManifold.jl delete mode 100644 test/testSOnxRnManifold.jl diff --git a/Project.toml b/Project.toml index a4ee4478..2b176e61 100644 --- a/Project.toml +++ b/Project.toml @@ -56,14 +56,14 @@ Dates = "1.10" DelimitedFiles = "1" DifferentialEquations = "7" Distributed = "1.10" -DistributedFactorGraphs = "0.25, 0.26" +DistributedFactorGraphs = "0.27" Distributions = "0.24, 0.25" DocStringExtensions = "0.8, 0.9" FileIO = "1" Flux = "0.14, 0.15, 0.16" ImageCore = "0.9, 0.10" ImageIO = "0.6" -IncrementalInference = "0.35" +IncrementalInference = "0.36" Interpolations = "0.14, 0.15" KernelDensityEstimate = "0.5.1, 0.6" LieGroups = "0.1.1" diff --git a/src/RoME.jl b/src/RoME.jl index 85190ee8..a5ae31db 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -66,6 +66,8 @@ include("entities/SpecialDefinitions.jl") #uses DFG v0.10.2 @defVariable for above include("services/FixmeManifolds.jl") +include("manifolds/SOnxRn_MetricManifold.jl") + include("variables/VariableTypes.jl") ## More factor types diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index a0ea88a4..58c7da9a 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -35,9 +35,10 @@ DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2; vect Pose2Pose2(::UniformScaling) = Pose2Pose2() function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) - G = getManifold(Pose2) - X̂ = log(base_manifold(G), getPointIdentity(G), LieGroups.compose(G, inv(G, p), q)) - return vee(LieAlgebra(G), X - X̂) + # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M + M = getManifold(Pose2) + X̂ = log(M, p, q) + return vee(M, p, X - X̂) # TODO check sign end # NOTE, serialization support -- will be reduced to macro in future diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index 340c2574..c44f106d 100644 --- a/src/factors/PriorPose2.jl +++ b/src/factors/PriorPose2.jl @@ -16,14 +16,6 @@ end DFG.getManifold(::InstanceType{PriorPose2}) = getManifold(Pose2) # SpecialEuclidean(2) -@inline function _vee(::typeof(SpecialEuclidean(2; vectors=HybridTangentRepresentation())), 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(::typeof(SpecialEuclidean(2; vectors=HybridTangentRepresentation())), 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) @@ -31,19 +23,13 @@ function (cf::CalcFactor{<:PriorPose2})(_m::AbstractArray{MT}, _p::AbstractArray return cf(m,p) end -# 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 M = getManifold(Pose2) - ϵ = getPointIdentity(Pose2) - # Xc = _vee(M, log(M, p, m)) - # X = log(M, p, m) - X = log(base_manifold(M), ϵ, LieGroups.compose(M, inv(M, p), m)) - return vee(LieAlgebra(M), X) + X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? + return vee(M, p, X) end #TODO Serialization of reference point p diff --git a/src/manifolds/SOnxRn_MetricManifold.jl b/src/manifolds/SOnxRn_MetricManifold.jl new file mode 100644 index 00000000..790ce63b --- /dev/null +++ b/src/manifolds/SOnxRn_MetricManifold.jl @@ -0,0 +1,161 @@ + + +# Left Invariant Rigid Body Kinematics Metric CrokeKumar eq 61. +# A family of left invariant metrics: +# G = [αI 0; 0 βI] +# where α and β are arbitrary constants, satisfies all the equations (80). +# This are the only left-invariant metrics which are compatible with the acceleration connection. +# Can we add α and β as parameters to the metric? +struct LeftInvariantKinematicMetric <: RiemannianMetric end + +SOnxRn_MetricManifold(n) = MetricManifold(SpecialEuclideanGroup(n; variant=:right), LeftInvariantKinematicMetric()) + +SOnxRn_MetricManifoldType = Union{typeof(SOnxRn_MetricManifold(2)), typeof(SOnxRn_MetricManifold(3))} + +# geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3 +function Manifolds.exp(M::SOnxRn_MetricManifoldType, p, X) + G = base_manifold(M) + ε = identity_element(M) #, typeof(p)) + return LieGroups.compose(G, p, exp(base_manifold(G), ε, X)) +end + +function Manifolds.exp!(M::SOnxRn_MetricManifoldType, q, p, X) + G = base_manifold(M) + ε = identity_element(M) #, typeof(p)) + return LieGroups.compose!(G, q, p, exp(base_manifold(G), ε, X)) +end + +function Manifolds.log(M::SOnxRn_MetricManifoldType, p, q) + G = base_manifold(M) + ε = identity_element(M) #, typeof(p)) + X = log(base_manifold(G), ε, LieGroups.compose(G, inv(G, p), q)) + return X +end + +function Manifolds.log!(M::SOnxRn_MetricManifoldType, X, p, q) + G = base_manifold(M) + ε = identity_element(M) #, typeof(p)) + log!(base_manifold(G), X, ε, LieGroups.compose(G, inv(G, p), q)) + return X +end + +function Manifolds.inner(M::SOnxRn_MetricManifoldType, p, X, Y) + Xtr = submanifold_components(M, X)[1] + XRo = submanifold_components(M, X)[2] + Ytr = submanifold_components(M, Y)[1] + YRo = submanifold_components(M, Y)[2] + # Metric on Chirikjian, p35 W = diagm([1,1,2]) + return dot(Xtr,Ytr) + dot(XRo, YRo)/2 +end + +function Manifolds.hat(M::SOnxRn_MetricManifoldType, p, X::AbstractVector, T=typeof(p)) + return hat(LieAlgebra(base_manifold(M)), X, T) +end + +function ManifoldsBase.get_vector( + M::SOnxRn_MetricManifoldType, g, c, B::AbstractBasis{<:Any,TangentSpaceType} +) + G = base_manifold(M) + return get_vector( + LieAlgebra(G), + c, + B; + tangent_vector_type=ManifoldsBase.tangent_vector_type(G, typeof(g)), + ) +end + +function ManifoldsBase.get_vector!( + M::SOnxRn_MetricManifoldType, X, g, c, B::AbstractBasis{<:Any,TangentSpaceType} +) + G = base_manifold(M) + return get_vector!(LieAlgebra(G), X, c, B) +end + +function ManifoldsBase.get_coordinates( + M::SOnxRn_MetricManifoldType, g, X, B::AbstractBasis{<:Any,TangentSpaceType} +) + G = base_manifold(M) + return get_coordinates(LieAlgebra(G), X, B) +end + +#FIXME: maybe replace this with identity_element +# call into base_manifold's identity_element, but got ambiguities. +Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) + + +@deprecate LieGroups.identity_element(G::LieGroup, p) identity_element(G, typeof(p)) false + +# FIXME why is this still needed, hopefully can be removed soon 🐛💥 +Base.convert(::Type{<:Tuple}, ::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) + + +## ======================================================================================= +## SE2 + metric +## TODO move to test file +if false +M = SOnxRn_MetricManifold(2) +G = base_manifold(M) +ε = identity_element(M) +T = typeof(identity_element(M)) +Xⁱ = [10, 1, pi/4] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) +q = compose(G, p, exp(base_manifold(G), ε, X)) + +q ≈ exp(M, p, X) +X ≈ log(M, p, q) + + +Xⁱ = [1, 1, 1] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) + +W = diagm([1,1,2]) + +XX = hat(LieAlgebra(G), Xⁱ) +0.5*tr(XX*W*XX') +inner(M, p, X, X) +inner(base_manifold(G), p, X, X) + +Manifolds.distance(M, ε, p) +Manifolds.distance(base_manifold(G), ε, p) + +## SE3 + metric + +M = SOnxRn_MetricManifold(3) +G = base_manifold(M) +ε = identity_element(M) +T = typeof(identity_element(M)) +Xⁱ = [10, 1, 0, 0, 0, pi/4] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) +q = compose(G, p, exp(base_manifold(G), ε, X)) + +q ≈ exp(M, p, X) +X ≈ log(M, p, q) + + +Xⁱ = [0, 0, 0, 0, 1, 1] +X = hat(LieAlgebra(G), Xⁱ, T) +p = exp(base_manifold(G), ε, X) + +0.5*tr(X.x[2]*X.x[2]') +dot(X.x[2], X.x[2]) + +inner(M, p, X, X) +inner(base_manifold(G), p, X, X) + +Manifolds.distance(M, ε, p) +Manifolds.distance(base_manifold(G), ε, p) + +Xⁱ = [1, 1, 0, 0, 1, 1] +W = diagm([1,1,1,2]) +X = hat(LieAlgebra(G), Xⁱ, T) +XX = hat(LieAlgebra(G), Xⁱ) +0.5*tr(XX*W*XX') +inner(M, p, X, X) +inner(base_manifold(G), p, X, X) + + +end \ No newline at end of file diff --git a/src/variables/VariableTypes.jl b/src/variables/VariableTypes.jl index a9ec7acc..790299e2 100644 --- a/src/variables/VariableTypes.jl +++ b/src/variables/VariableTypes.jl @@ -32,7 +32,7 @@ $(TYPEDEF) Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM. """ -@defVariable Pose2 SpecialEuclideanGroup(2; variant=:right) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +@defVariable Pose2 SOnxRn_MetricManifold(2) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) """ $(TYPEDEF) diff --git a/test/testSOnxRnManifold.jl b/test/testSOnxRnManifold.jl deleted file mode 100644 index c29323fd..00000000 --- a/test/testSOnxRnManifold.jl +++ /dev/null @@ -1,111 +0,0 @@ - - -# Rigid Body Kinematics Metric CrokeKumar eq 61. -# A family of left invariant metrics: -# G = [αI 0; 0 βI] -# where α and β are arbitrary constants, satisfies all the equations (80). -# This are the only left-invariant metrics which are compatible with the acceleration connection. -# Can we add α and β as parameters to the metric? -struct LeftInvariantRBKMetric <: RiemannianMetric end - -SOnxRnManifold(n) = MetricManifold(SpecialEuclideanGroup(n; variant=:right), LeftInvariantRBKMetric()) - -SOnxRnManifoldType = Union{typeof(SOnxRnManifold(2)), typeof(SOnxRnManifold(3))} - -# geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3 -function Manifolds.exp(M::SOnxRnManifoldType, p, X) - G = base_manifold(M) - ε = identity_element(M, typeof(p)) - return compose(G, p, exp(base_manifold(G), ε, X)) -end - -function Manifolds.log(M::SOnxRnManifoldType, p, q) - G = base_manifold(M) - ε = identity_element(M, typeof(p)) - X = log(base_manifold(G), ε, compose(G, inv(G, p), q)) - return X -end - -function Manifolds.inner(M::SOnxRnManifoldType, p, X, Y) - Xtr = submanifold_components(M, X)[1] - XRo = submanifold_components(M, X)[2] - Ytr = submanifold_components(M, Y)[1] - YRo = submanifold_components(M, Y)[2] - # Metric on Chirikjian, p35 W = diagm([1,1,2]) - return dot(Xtr,Ytr) + dot(XRo, YRo)/2 -end - - - - - - - - -## ======================================================================================= -## SE2 + metric - -M = SOnxRnManifold(2) -G = base_manifold(M) -ε = identity_element(M) -T = typeof(identity_element(M)) -Xⁱ = [10, 1, pi/4] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) -q = compose(G, p, exp(base_manifold(G), ε, X)) - -q ≈ exp(M, p, X) -X ≈ log(M, p, q) - - -Xⁱ = [1, 1, 1] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) - -W = diagm([1,1,2]) - -XX = hat(LieAlgebra(G), Xⁱ) -0.5*tr(XX*W*XX') -inner(M, p, X, X) -inner(base_manifold(G), p, X, X) - -Manifolds.distance(M, ε, p) -Manifolds.distance(base_manifold(G), ε, p) - -## SE3 + metric - -M = SOnxRnManifold(3) -G = base_manifold(M) -ε = identity_element(M) -T = typeof(identity_element(M)) -Xⁱ = [10, 1, 0, 0, 0, pi/4] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) -q = compose(G, p, exp(base_manifold(G), ε, X)) - -q ≈ exp(M, p, X) -X ≈ log(M, p, q) - - -Xⁱ = [0, 0, 0, 0, 1, 1] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) - -0.5*tr(X.x[2]*X.x[2]') -dot(X.x[2], X.x[2]) - -inner(M, p, X, X) -inner(base_manifold(G), p, X, X) - -Manifolds.distance(M, ε, p) -Manifolds.distance(base_manifold(G), ε, p) - -Xⁱ = [1, 1, 0, 0, 1, 1] -W = diagm([1,1,1,2]) -X = hat(LieAlgebra(G), Xⁱ, T) -XX = hat(LieAlgebra(G), Xⁱ) -0.5*tr(XX*W*XX') -inner(M, p, X, X) -inner(base_manifold(G), p, X, X) - - From c049f8b98a2f66a3bea17fe11c7728011b85fc69 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 1 Jul 2025 11:53:03 +0200 Subject: [PATCH 07/18] fixme on piracy --- src/manifolds/SOnxRn_MetricManifold.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/manifolds/SOnxRn_MetricManifold.jl b/src/manifolds/SOnxRn_MetricManifold.jl index 790ce63b..06b37b9f 100644 --- a/src/manifolds/SOnxRn_MetricManifold.jl +++ b/src/manifolds/SOnxRn_MetricManifold.jl @@ -83,7 +83,7 @@ end Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) - +#FIXME remove, only temporary workaround as first signiture is used in many places @deprecate LieGroups.identity_element(G::LieGroup, p) identity_element(G, typeof(p)) false # FIXME why is this still needed, hopefully can be removed soon 🐛💥 From 9a2ad1e75b3506b8e73d0279f16038adb834e23b Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Thu, 29 Aug 2024 17:16:22 +0200 Subject: [PATCH 08/18] Split Variable tipes to RoMETypes --- RoMETypes/Project.toml | 18 +++++++++++++ RoMETypes/src/RoMETypes.jl | 25 +++++++++++++++++++ .../src}/variables/VariableTypes.jl | 16 ------------ src/RoME.jl | 2 ++ src/services/FixmeManifolds.jl | 3 +++ 5 files changed, 48 insertions(+), 16 deletions(-) create mode 100644 RoMETypes/Project.toml create mode 100644 RoMETypes/src/RoMETypes.jl rename {src => RoMETypes/src}/variables/VariableTypes.jl (90%) diff --git a/RoMETypes/Project.toml b/RoMETypes/Project.toml new file mode 100644 index 00000000..d9cb7aa4 --- /dev/null +++ b/RoMETypes/Project.toml @@ -0,0 +1,18 @@ +name = "RoMETypes" +uuid = "298dcef3-e3bd-403a-988f-2dd5ca311a3f" +authors = ["Johannes Terblanche "] +version = "0.1.0" + +[deps] +DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04" +DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" +Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" +RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" +StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" + +[compat] +DistributedFactorGraphs = "0.24.0" +DocStringExtensions = "0.9.3" +Manifolds = "0.9.20" +RecursiveArrayTools = "3.27.0" +StaticArrays = "1.9.7" diff --git a/RoMETypes/src/RoMETypes.jl b/RoMETypes/src/RoMETypes.jl new file mode 100644 index 00000000..28345666 --- /dev/null +++ b/RoMETypes/src/RoMETypes.jl @@ -0,0 +1,25 @@ +module RoMETypes + +using DistributedFactorGraphs +using DocStringExtensions +using Manifolds +using RecursiveArrayTools +using StaticArrays + +import DistributedFactorGraphs: getVariableType, AbstractManifoldMinimize + +export + Point2, + Point3, + Pose2, + Pose3, + Rotation3, + RotVelPos, + VelPos3, + DynPoint2, + DynPose2, + projectCartesian + +include("variables/VariableTypes.jl") + +end # module RoMETypes diff --git a/src/variables/VariableTypes.jl b/RoMETypes/src/variables/VariableTypes.jl similarity index 90% rename from src/variables/VariableTypes.jl rename to RoMETypes/src/variables/VariableTypes.jl index 790299e2..e58482b7 100644 --- a/src/variables/VariableTypes.jl +++ b/RoMETypes/src/variables/VariableTypes.jl @@ -1,10 +1,3 @@ -# TODO integration underway with Manifolds.jl, see RoME #244, also see IIF #467 regarding consolidation effort. - -import DistributedFactorGraphs: getVariableType - -export projectCartesian - - """ $(TYPEDEF) @@ -128,12 +121,3 @@ projectCartesian(pose::Union{<:Point2,<:Point3,<:Pose2,<:Pose3,<:DynPoint2,<:Dyn # - -# Still experimental -# export BearingRange2 -@defVariable BearingRange2 BearingRange_Manifold ArrayPartition(0.0,0.0) - - - - -# diff --git a/src/RoME.jl b/src/RoME.jl index a5ae31db..6a1bbd3e 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -56,6 +56,8 @@ using OrderedCollections: OrderedDict # const AMP = ApproxManifoldProducts +include("../RoMETypes/src/RoMETypes.jl") +using ..RoMETypes # export the API include("ExportAPI.jl") diff --git a/src/services/FixmeManifolds.jl b/src/services/FixmeManifolds.jl index 963dcf2c..dda2dd49 100644 --- a/src/services/FixmeManifolds.jl +++ b/src/services/FixmeManifolds.jl @@ -76,6 +76,9 @@ function Statistics.mean(::typeof(BearingRange_Manifold), pts::AbstractVector) return [mc; mr] end +# Still experimental +# export BearingRange2 +@defVariable BearingRange2 BearingRange_Manifold ArrayPartition(0.0,0.0) From a5bb9e5241e8a0d7e5bfa538ea7373d8dd09ed14 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Mon, 30 Jun 2025 15:53:30 +0200 Subject: [PATCH 09/18] Move Points and Poses --- RoMETypes/Project.toml | 4 +- RoMETypes/src/RoMETypes.jl | 11 ++ RoMETypes/src/factors/FactorTypes.jl | 64 +++++++++++ src/RoME.jl | 9 +- src/factors/Point2D.jl | 78 ------------- src/factors/Point3D.jl | 41 ------- src/factors/Point3Point3.jl | 33 ------ src/factors/Points.jl | 16 +++ src/factors/Pose3D.jl | 36 ------ src/factors/Pose3Pose3.jl | 127 --------------------- src/factors/Poses.jl | 164 +++++++++++++++++++++++++++ 11 files changed, 259 insertions(+), 324 deletions(-) create mode 100644 RoMETypes/src/factors/FactorTypes.jl delete mode 100644 src/factors/Point2D.jl delete mode 100644 src/factors/Point3D.jl delete mode 100644 src/factors/Point3Point3.jl create mode 100644 src/factors/Points.jl delete mode 100644 src/factors/Pose3D.jl delete mode 100644 src/factors/Pose3Pose3.jl create mode 100644 src/factors/Poses.jl diff --git a/RoMETypes/Project.toml b/RoMETypes/Project.toml index d9cb7aa4..9a577f59 100644 --- a/RoMETypes/Project.toml +++ b/RoMETypes/Project.toml @@ -11,8 +11,8 @@ RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" [compat] -DistributedFactorGraphs = "0.24.0" +DistributedFactorGraphs = "0.27.0" DocStringExtensions = "0.9.3" -Manifolds = "0.9.20" +Manifolds = "0.10" RecursiveArrayTools = "3.27.0" StaticArrays = "1.9.7" diff --git a/RoMETypes/src/RoMETypes.jl b/RoMETypes/src/RoMETypes.jl index 28345666..c2b7ae0c 100644 --- a/RoMETypes/src/RoMETypes.jl +++ b/RoMETypes/src/RoMETypes.jl @@ -20,6 +20,17 @@ export DynPose2, projectCartesian +export + PriorPoint2, + PackedPriorPoint2, + PriorPoint3, + PackedPriorPoint3, + Pose2Pose2, + PackedPose2Pose2, + Pose3Pose3, + PackedPose3Pose3 + include("variables/VariableTypes.jl") +include("factors/FactorTypes.jl") end # module RoMETypes diff --git a/RoMETypes/src/factors/FactorTypes.jl b/RoMETypes/src/factors/FactorTypes.jl new file mode 100644 index 00000000..322f9343 --- /dev/null +++ b/RoMETypes/src/factors/FactorTypes.jl @@ -0,0 +1,64 @@ +# ======================================================================================== +# Pose +# ======================================================================================== + +""" +$(TYPEDEF) + +Direction observation information of a `Point3` variable. +""" +DFG.@defFactorType PriorPoint3 DFG.AbstractPrior Manifolds.TranslationGroup(3) +DFG.@defFactorType Point3Point3 AbstractManifoldMinimize Manifolds.TranslationGroup(3) + + +DFG.@defFactorType Point2Point2 AbstractManifoldMinimize Manifolds.TranslationGroup(2) +DFG.@defFactorType PriorPoint2 DFG.AbstractPrior Manifolds.TranslationGroup(2) + +# ======================================================================================== +# Pose +# ======================================================================================== + +""" +$(TYPEDEF) + +Rigid transform between two Pose2's, assuming (x,y,theta). + +Calcuated as: +```math +\\begin{aligned} +\\hat{q}=\\exp_pX_m\\\\ +X = \\log_q \\hat{q}\\\\ +X^i = \\mathrm{vee}(q, X) +\\end{aligned} +``` +with: +``\\mathcal M= \\mathrm{SE}(2)`` Special Euclidean group\\ +``p`` and ``q`` ``\\in \\mathcal M`` the two Pose2 points\\ +the measurement vector ``X_m \\in T_p \\mathcal M``\\ +and the error vector ``X \\in T_q \\mathcal M``\\ +``X^i`` coordinates of ``X`` + +DevNotes +- Maybe with Manifolds.jl, `{T <: IIF.SamplableBelief, S, R, P}` + +Related + +[`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref) +""" +DFG.@defFactorType Pose2Pose2 AbstractManifoldMinimize Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) + +""" +$(TYPEDEF) + +Introduce direct observations on all dimensions of a Pose2 variable: + +Example: +-------- +```julia +PriorPose2( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) +``` +""" +DFG.@defFactorType PriorPose2 AbstractPrior Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) + +DFG.@defFactorType Pose3Pose3 AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +DFG.@defFactorType PriorPose3 AbstractPrior Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) diff --git a/src/RoME.jl b/src/RoME.jl index 6a1bbd3e..a8fa0a0f 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -74,15 +74,14 @@ include("variables/VariableTypes.jl") ## More factor types # RoME internal factors (FYI outside factors are easy, see Caesar documentation) -include("factors/Point2D.jl") +include("factors/Points.jl") +include("factors/Poses.jl") include("factors/Range2D.jl") include("factors/Bearing2D.jl") include("factors/BearingRange2D.jl") include("factors/Polar.jl") include("factors/PriorVelPos3.jl") -include("factors/PriorPose2.jl") include("factors/PartialPriorPose2.jl") -include("factors/Pose2D.jl") include("factors/Pose2Point2.jl") include("factors/MutablePose2Pose2.jl") include("factors/DynPoint2D.jl") @@ -91,10 +90,6 @@ include("factors/VelPosRotVelPos.jl") include("factors/DynPose2D.jl") include("factors/VelPose2D.jl") include("factors/VelAlign.jl") -include("factors/Point3D.jl") -include("factors/Point3Point3.jl") -include("factors/Pose3D.jl") -include("factors/Pose3Pose3.jl") include("factors/PartialPose3.jl") include("factors/MultipleFeaturesConstraint.jl") include("factors/InertialPose3.jl") diff --git a/src/factors/Point2D.jl b/src/factors/Point2D.jl deleted file mode 100644 index 86f83f4c..00000000 --- a/src/factors/Point2D.jl +++ /dev/null @@ -1,78 +0,0 @@ - -""" -$(TYPEDEF) - -Direction observation information of a `Point2` variable. -""" -Base.@kwdef struct PriorPoint2{T <: IIF.SamplableBelief} <: IncrementalInference.AbstractPrior - Z::T = MvNormal(zeros(2),LinearAlgebra.diagm([0.01;0.01])) -end - -DFG.getManifold(::InstanceType{PriorPoint2}) = getManifold(Point2) - - -function (cf::CalcFactor{<:PriorPoint2})(meas, - X1) - # - return meas[1:2] .- X1[1:2] -end - -""" -$(TYPEDEF) -""" -Base.@kwdef struct Point2Point2{D <: IIF.SamplableBelief} <: AbstractManifoldMinimize #RelativeRoots - Z::D = MvNormal(zeros(2),LinearAlgebra.diagm([0.1;0.1])) -end - -getManifold(::InstanceType{Point2Point2}) = getManifold(Point2) - - -function (pp2r::CalcFactor{<:Point2Point2})(meas, - xi, - xj ) - # - return meas[1:2] .- (xj[1:2] .- xi[1:2]) -end - - - - -# --------------------------------------------------------- - - - -""" -$(TYPEDEF) - -Serialization type for `PriorPoint2`. -""" -Base.@kwdef struct PackedPriorPoint2 <: AbstractPackedFactor - Z::PackedSamplableBelief -end - -function convert(::Type{PriorPoint2}, d::PackedPriorPoint2) - return PriorPoint2(convert(SamplableBelief, d.Z)) -end -function convert(::Type{PackedPriorPoint2}, d::PriorPoint2) - return PackedPriorPoint2(convert(PackedSamplableBelief, d.Z)) -end - - - - - -""" -$(TYPEDEF) - -Serialization type for `Point2Point2`. -""" -Base.@kwdef struct PackedPoint2Point2 <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{Point2Point2}, d::PackedPoint2Point2) - return Point2Point2( convert(SamplableBelief, d.Z) ) -end -function convert(::Type{PackedPoint2Point2}, d::Point2Point2) - return PackedPoint2Point2( convert(PackedSamplableBelief, d.Z) ) -end - diff --git a/src/factors/Point3D.jl b/src/factors/Point3D.jl deleted file mode 100644 index 38a39870..00000000 --- a/src/factors/Point3D.jl +++ /dev/null @@ -1,41 +0,0 @@ - - -""" -$(TYPEDEF) - -Direction observation information of a `Point3` variable. -""" -Base.@kwdef struct PriorPoint3{T} <: IncrementalInference.AbstractPrior where {T <: IIF.SamplableBelief} - Z::T = MvNormal(zeros(3),diagm([1;1;1.0])) -end - -getManifold(::InstanceType{PriorPoint3}) = getManifold(Point3) - -# PriorPoint3 aka PriorPose3XYZ -function (cf::CalcFactor{<:PriorPoint3})(meas, X1::ArrayPartition) - Xc::SVector{3} = meas - X1.x[1] - return Xc -end - -function (cf::CalcFactor{<:PriorPoint3})(meas, X1::AbstractVector) - # return meas[1:3] .- X1[1:3] - return meas - X1 -end - - -## Serialized packing type and converters - -""" -$(TYPEDEF) -""" -Base.@kwdef struct PackedPriorPoint3 <: AbstractPackedFactor - Z::PackedSamplableBelief -end - - -function convert(::Type{PriorPoint3}, d::PackedPriorPoint3) - return PriorPoint3(convert(SamplableBelief, d.Z)) -end -function convert(::Type{PackedPriorPoint3}, d::PriorPoint3) - return PackedPriorPoint3(convert(PackedSamplableBelief, d.Z)) -end diff --git a/src/factors/Point3Point3.jl b/src/factors/Point3Point3.jl deleted file mode 100644 index 392d3cf6..00000000 --- a/src/factors/Point3Point3.jl +++ /dev/null @@ -1,33 +0,0 @@ - - -""" -$(TYPEDEF) -""" -Base.@kwdef struct Point3Point3{D <: IIF.SamplableBelief} <: AbstractRelativeMinimize - Z::D = MvNormal(zeros(3),diagm([0.1;0.1;0.1])) -end - -getManifold(::InstanceType{Point3Point3}) = getManifold(Point3) - -function (cf::CalcFactor{<:Point3Point3})(meas, - xi, - xj ) - # - return meas[1:3] .- (xj[1:3] .- xi[1:3]) -end - - -""" -$(TYPEDEF) - -Serialization type for `Point3Point3`. -""" -Base.@kwdef struct PackedPoint3Point3 <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{Point3Point3}, d::PackedPoint3Point3) - return Point3Point3( convert(SamplableBelief, d.Z) ) -end -function convert(::Type{PackedPoint3Point3}, d::Point3Point3) - return PackedPoint3Point3( convert(PackedSamplableBelief, d.Z) ) -end \ No newline at end of file diff --git a/src/factors/Points.jl b/src/factors/Points.jl new file mode 100644 index 00000000..ce743711 --- /dev/null +++ b/src/factors/Points.jl @@ -0,0 +1,16 @@ + +(cf::CalcFactor{<:PriorPoint2})(X, p) = X[1:2] .- p[1:2] + +(pp2r::CalcFactor{<:Point2Point2})(X, p, q) = X[1:2] .- (q[1:2] .- p[1:2]) + +function (cf::CalcFactor{<:PriorPoint3})(X, p::ArrayPartition) + Xc::SVector{3} = X - p.x[1] + return Xc +end + +(cf::CalcFactor{<:PriorPoint3})(X, p::AbstractVector) = X - p + +function (cf::CalcFactor{<:Point3Point3})(X, p, q) + # + return X[1:3] .- (q[1:3] .- p[1:3]) +end diff --git a/src/factors/Pose3D.jl b/src/factors/Pose3D.jl deleted file mode 100644 index f096492e..00000000 --- a/src/factors/Pose3D.jl +++ /dev/null @@ -1,36 +0,0 @@ - -# regular prior for Pose3 - -""" -$(TYPEDEF) - -Direct observation information of `Pose3` variable type. -""" -Base.@kwdef struct PriorPose3{T <: IIF.SamplableBelief} <: IncrementalInference.AbstractPrior - Z::T = MvNormal(zeros(6), diagm([0.01*ones(3);0.0001*ones(3)])) -end - -getManifold(::InstanceType{PriorPose3}) = getManifold(Pose3) # SpecialEuclidean(3) - -function (cf::CalcFactor{<:PriorPose3})(m, p) - M = getManifold(Pose3) - Xc = vee(M, p, log(M, p, m)) - return Xc -end - - -#FIXME Serialization -""" -$(TYPEDEF) - -Serialization type for PriorPose3. -""" -Base.@kwdef struct PackedPriorPose3 <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{PriorPose3}, packed::PackedPriorPose3) - return PriorPose3( convert(SamplableBelief, packed.Z) ) -end -function convert(::Type{PackedPriorPose3}, obj::PriorPose3) - return PackedPriorPose3(convert(PackedSamplableBelief, obj.Z)) -end diff --git a/src/factors/Pose3Pose3.jl b/src/factors/Pose3Pose3.jl deleted file mode 100644 index 88360a8e..00000000 --- a/src/factors/Pose3Pose3.jl +++ /dev/null @@ -1,127 +0,0 @@ -# Pose3Pose3 evaluation functions - -# ------------------------------------ -""" -$(TYPEDEF) - -Rigid transform factor between two Pose3 compliant variables. -""" -Base.@kwdef struct Pose3Pose3{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize - Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)])) -end - -getManifold(::InstanceType{Pose3Pose3}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3) - -Pose3Pose3(::UniformScaling) = Pose3Pose3() - -function (cf::CalcFactor{<:Pose3Pose3})(X, p::ArrayPartition{T}, q) where T - M = getManifold(Pose3) - # X: p_Xq̂ - # p: w_H_p - # q: w_H_q - # ̂q: w_H_̂q = w_H_p * exp_0(p_Xq̂) - # w_H_̂q = Manifolds.compose(M, w_H_p, exp(M, getPointIdentity(M), p_Xq̂)) - - q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) - - Xc::SVector{6,T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) - return Xc -end - -# function (cf::CalcFactor{<:Pose3Pose3})(X, p, q) -# M = cf.manifold # getManifold(Pose3) -# ϵX = exp(M, getPointIdentity(M), X) -# q̂ = ArrayPartition(p.x[2]*ϵX.x[1] + p.x[1], p.x[2]*ϵX.x[2]) -# Xc = vee(M, q, log(M, q, q̂)) -# return Xc -# end - -#TODO Serialization - -""" -$(TYPEDEF) - -Serialization type for `Pose3Pose3`. -""" -Base.@kwdef struct PackedPose3Pose3 <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{Pose3Pose3}, packed::PackedPose3Pose3) - return Pose3Pose3( convert(SamplableBelief, packed.Z) ) -end -function convert(::Type{PackedPose3Pose3}, obj::Pose3Pose3) - return PackedPose3Pose3( convert(PackedSamplableBelief, obj.Z) ) -end - -## -Base.@kwdef struct Pose3Pose3RotOffset{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize - Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)])) -end - -getManifold(::InstanceType{Pose3Pose3RotOffset}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3) - - -# measurement is in frame a, for example imu frame -# p and q is in frame b, for example body frame -# bRa is the rotation to get a in the b frame -# measurement in frame a is converted to frame b and used to calculate the error -function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa) - M = getManifold(Pose3Pose3RotOffset) - # measurement in frame a, input is tangent, can also use vector transport - a_m = exp(M, getPointIdentity(M), aX) - b_m = ArrayPartition(a_m.x[1], bRa * a_m.x[2]) - - q̂ = Manifolds.compose(M, p, b_m) - return vee(M, q, log(M, q, q̂)) # coordinates -end - - -## -Base.@kwdef struct Pose3Pose3Transform{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize - Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)])) -end - -getManifold(::InstanceType{Pose3Pose3Transform}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3) - -function (cf::CalcFactor{<:Pose3Pose3Transform})(p_NX, p, q, Δ) - - M = getManifold(Pose3Pose3Transform) - ε = getPointIdentity(M) - - Δn = compose(M, Δ, exp(M, ε, p_NX)) - q̂ = Manifolds.compose(M, p, Δn) - - Xc::SVector{6,T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) - return Xc -end - -## ==================================== -## Pose3Pose3UnitTrans Factor - -""" - $(TYPEDEF) -Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation. -""" -Base.@kwdef struct Pose3Pose3UnitTrans{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize - Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)])) -end - -getManifold(::InstanceType{Pose3Pose3UnitTrans}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3) - -function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where T - M = getManifold(Pose3) - q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) - Xc::SVector{6,T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) - return SVector{6,T}(normalize(Xc[1:3])..., Xc[4:6]...) -end - -Base.@kwdef struct PackedPose3Pose3UnitTrans <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{Pose3Pose3UnitTrans}, packed::PackedPose3Pose3UnitTrans) - return Pose3Pose3UnitTrans( convert(SamplableBelief, packed.Z) ) -end -function convert(::Type{PackedPose3Pose3UnitTrans}, obj::Pose3Pose3UnitTrans) - return PackedPose3Pose3UnitTrans( convert(PackedSamplableBelief, obj.Z) ) -end - diff --git a/src/factors/Poses.jl b/src/factors/Poses.jl new file mode 100644 index 00000000..b0bc4d11 --- /dev/null +++ b/src/factors/Poses.jl @@ -0,0 +1,164 @@ + +#TODO deprecate, only use LieGroups +@inline function _vee( + ::typeof(SpecialEuclidean(2; vectors = HybridTangentRepresentation())), + 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 + +#TODO deprecate, only use LieGroups +@inline function _compose( + ::typeof(SpecialEuclidean(2; vectors = HybridTangentRepresentation())), + 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 + +# 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} + M = getManifold(Pose2) + ϵ = getPointIdentity(M) + Xc = _vee(M, log(M, p, m)) + # X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m)) + # Xc = vee(M, ϵ, X) + return Xc +end + +## NOTE likely deprecated comparitors, see DFG compareFields, compareAll instead +compare(a::PriorPose2, b::PriorPose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) + +# Assumes X is a tangent vector +function (cf::CalcFactor{<:Pose2Pose2})( + _X::AbstractArray{MT}, + _p::AbstractArray{PT}, + _q::AbstractArray{LT}, +) where {MT, PT, LT} + #TODO remove this convertions + # @warn "This warning should not be triggered after StaticArrays upgrade" maxlog=10 + 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} + M = getManifold(Pose2) + # ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I)) + ϵ0 = getPointIdentity(M) + + ϵX = exp(M, ϵ0, X) + # q̂ = Manifolds.compose(M, p, ϵX) + q̂ = _compose(M, p, ϵX) + X_hat = log(M, q, q̂)#::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}} + # Xc = vee(M, q, X_hat) + Xc = _vee(M, X_hat)#::SVector{3,T} + return Xc +end + +# FIXME, rather have separate compareDensity functions +compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) + +# + +# regular prior for Pose3 + +function (cf::CalcFactor{<:PriorPose3})(m, p) + M = getManifold(Pose3) + Xc = vee(M, p, log(M, p, m)) + return Xc +end + +# Pose3Pose3 evaluation functions + +function (cf::CalcFactor{<:Pose3Pose3})(X, p::ArrayPartition{T}, q) where {T} + M = getManifold(Pose3) + # X: p_Xq̂ + # p: w_H_p + # q: w_H_q + # ̂q: w_H_̂q = w_H_p * exp_0(p_Xq̂) + # w_H_̂q = Manifolds.compose(M, w_H_p, exp(M, getPointIdentity(M), p_Xq̂)) + + q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) + + Xc::SVector{6, T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) + return Xc +end + +# function (cf::CalcFactor{<:Pose3Pose3})(X, p, q) +# M = cf.manifold # getManifold(Pose3) +# ϵX = exp(M, getPointIdentity(M), X) +# q̂ = ArrayPartition(p.x[2]*ϵX.x[1] + p.x[1], p.x[2]*ϵX.x[2]) +# Xc = vee(M, q, log(M, q, q̂)) +# return Xc +# end + +## +#TODO is this manifold not SO3 +DFG.@defFactorType Pose3Pose3RotOffset AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) + +# measurement is in frame a, for example imu frame +# p and q is in frame b, for example body frame +# bRa is the rotation to get a in the b frame +# measurement in frame a is converted to frame b and used to calculate the error +function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa) + M = getManifold(Pose3Pose3RotOffset) + # measurement in frame a, input is tangent, can also use vector transport + a_m = exp(M, getPointIdentity(M), aX) + b_m = ArrayPartition(a_m.x[1], bRa * a_m.x[2]) + + q̂ = Manifolds.compose(M, p, b_m) + return vee(M, q, log(M, q, q̂)) # coordinates +end + +## +DFG.@defFactorType Pose3Pose3Transform AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) + +function (cf::CalcFactor{<:Pose3Pose3Transform})(p_NX, p, q, Δ) + M = getManifold(Pose3Pose3Transform) + ε = getPointIdentity(M) + + Δn = compose(M, Δ, exp(M, ε, p_NX)) + q̂ = Manifolds.compose(M, p, Δn) + + Xc::SVector{6, T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) + return Xc +end + +## ==================================== +## Pose3Pose3UnitTrans Factor + +""" + $(TYPEDEF) +Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation. +""" +DFG.@defFactorType Pose3Pose3UnitTrans AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) + +function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where {T} + M = getManifold(Pose3) + q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) + Xc::SVector{6, T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) + return SVector{6, T}(normalize(Xc[1:3])..., Xc[4:6]...) +end From 2513cdc5982d099cfbb30f8b62eb613273f29df1 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 1 Jul 2025 23:00:54 +0200 Subject: [PATCH 10/18] missed exports --- RoMETypes/src/RoMETypes.jl | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/RoMETypes/src/RoMETypes.jl b/RoMETypes/src/RoMETypes.jl index c2b7ae0c..08cc8961 100644 --- a/RoMETypes/src/RoMETypes.jl +++ b/RoMETypes/src/RoMETypes.jl @@ -28,7 +28,15 @@ export Pose2Pose2, PackedPose2Pose2, Pose3Pose3, - PackedPose3Pose3 + PackedPose3Pose3, + PriorPose3, + PackedPriorPose3, + Point3Point3, + PackedPoint3Point3, + Point2Point2, + PackedPoint2Point2, + PriorPose2, + PackedPriorPose2 include("variables/VariableTypes.jl") include("factors/FactorTypes.jl") From 6c7259a101c6c1bbdff04d8cd3fd3e41389f799a Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 8 Jul 2025 10:09:54 +0200 Subject: [PATCH 11/18] use defObservationType --- RoMETypes/src/factors/FactorTypes.jl | 16 ++++++++-------- src/factors/Poses.jl | 6 +++--- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/RoMETypes/src/factors/FactorTypes.jl b/RoMETypes/src/factors/FactorTypes.jl index 322f9343..5c72724f 100644 --- a/RoMETypes/src/factors/FactorTypes.jl +++ b/RoMETypes/src/factors/FactorTypes.jl @@ -7,12 +7,12 @@ $(TYPEDEF) Direction observation information of a `Point3` variable. """ -DFG.@defFactorType PriorPoint3 DFG.AbstractPrior Manifolds.TranslationGroup(3) -DFG.@defFactorType Point3Point3 AbstractManifoldMinimize Manifolds.TranslationGroup(3) +DFG.@defObservationType PriorPoint3 DFG.AbstractPrior Manifolds.TranslationGroup(3) +DFG.@defObservationType Point3Point3 AbstractManifoldMinimize Manifolds.TranslationGroup(3) -DFG.@defFactorType Point2Point2 AbstractManifoldMinimize Manifolds.TranslationGroup(2) -DFG.@defFactorType PriorPoint2 DFG.AbstractPrior Manifolds.TranslationGroup(2) +DFG.@defObservationType Point2Point2 AbstractManifoldMinimize Manifolds.TranslationGroup(2) +DFG.@defObservationType PriorPoint2 DFG.AbstractPrior Manifolds.TranslationGroup(2) # ======================================================================================== # Pose @@ -45,7 +45,7 @@ Related [`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref) """ -DFG.@defFactorType Pose2Pose2 AbstractManifoldMinimize Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +DFG.@defObservationType Pose2Pose2 AbstractManifoldMinimize Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) """ $(TYPEDEF) @@ -58,7 +58,7 @@ Example: PriorPose2( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) ``` """ -DFG.@defFactorType PriorPose2 AbstractPrior Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +DFG.@defObservationType PriorPose2 AbstractPrior Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) -DFG.@defFactorType Pose3Pose3 AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) -DFG.@defFactorType PriorPose3 AbstractPrior Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3 AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +DFG.@defObservationType PriorPose3 AbstractPrior Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) diff --git a/src/factors/Poses.jl b/src/factors/Poses.jl index b0bc4d11..f0bfea4c 100644 --- a/src/factors/Poses.jl +++ b/src/factors/Poses.jl @@ -117,7 +117,7 @@ end ## #TODO is this manifold not SO3 -DFG.@defFactorType Pose3Pose3RotOffset AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3RotOffset AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) # measurement is in frame a, for example imu frame # p and q is in frame b, for example body frame @@ -134,7 +134,7 @@ function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa) end ## -DFG.@defFactorType Pose3Pose3Transform AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3Transform AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) function (cf::CalcFactor{<:Pose3Pose3Transform})(p_NX, p, q, Δ) M = getManifold(Pose3Pose3Transform) @@ -154,7 +154,7 @@ end $(TYPEDEF) Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation. """ -DFG.@defFactorType Pose3Pose3UnitTrans AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3UnitTrans AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where {T} M = getManifold(Pose3) From 5a1ee0bae19a7d18e50643b294d0d700df955192 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Mon, 11 Aug 2025 18:22:28 +0200 Subject: [PATCH 12/18] Manually merge conflics in Pose2Pose2 and PriorPose2 --- src/factors/Poses.jl | 89 ++++++-------------------------------------- 1 file changed, 11 insertions(+), 78 deletions(-) diff --git a/src/factors/Poses.jl b/src/factors/Poses.jl index f0bfea4c..cb2e3a3b 100644 --- a/src/factors/Poses.jl +++ b/src/factors/Poses.jl @@ -1,21 +1,4 @@ -#TODO deprecate, only use LieGroups -@inline function _vee( - ::typeof(SpecialEuclidean(2; vectors = HybridTangentRepresentation())), - 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 - -#TODO deprecate, only use LieGroups -@inline function _compose( - ::typeof(SpecialEuclidean(2; vectors = HybridTangentRepresentation())), - 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}, @@ -26,63 +9,28 @@ function (cf::CalcFactor{<:PriorPose2})( return cf(m, p) end -# 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} - M = getManifold(Pose2) - ϵ = getPointIdentity(M) - Xc = _vee(M, log(M, p, m)) - # X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m)) - # Xc = vee(M, ϵ, X) - return Xc + M = getManifold(PriorPose2) + X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? + return vee(M, p, X) end ## NOTE likely deprecated comparitors, see DFG compareFields, compareAll instead compare(a::PriorPose2, b::PriorPose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) -# Assumes X is a tangent vector -function (cf::CalcFactor{<:Pose2Pose2})( - _X::AbstractArray{MT}, - _p::AbstractArray{PT}, - _q::AbstractArray{LT}, -) where {MT, PT, LT} - #TODO remove this convertions - # @warn "This warning should not be triggered after StaticArrays upgrade" maxlog=10 - 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} - M = getManifold(Pose2) - # ϵ0 = ArrayPartition(zeros(SVector{2,T}), SMatrix{2, 2, T}(I)) - ϵ0 = getPointIdentity(M) - - ϵX = exp(M, ϵ0, X) - # q̂ = Manifolds.compose(M, p, ϵX) - q̂ = _compose(M, p, ϵX) - X_hat = log(M, q, q̂)#::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}} - # Xc = vee(M, q, X_hat) - Xc = _vee(M, X_hat)#::SVector{3,T} - return Xc +function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) + # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M + M = getManifold(Pose2) + X̂ = log(M, p, q) + return vee(M, p, X - X̂) # TODO check sign end # FIXME, rather have separate compareDensity functions compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) -# - # regular prior for Pose3 function (cf::CalcFactor{<:PriorPose3})(m, p) @@ -94,27 +42,12 @@ end # Pose3Pose3 evaluation functions function (cf::CalcFactor{<:Pose3Pose3})(X, p::ArrayPartition{T}, q) where {T} - M = getManifold(Pose3) - # X: p_Xq̂ - # p: w_H_p - # q: w_H_q - # ̂q: w_H_̂q = w_H_p * exp_0(p_Xq̂) - # w_H_̂q = Manifolds.compose(M, w_H_p, exp(M, getPointIdentity(M), p_Xq̂)) - - q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) - - Xc::SVector{6, T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) + M = getManifold(Pose3Pose3) + X̂ = log(M, p, q) + Xc::SVector{6, T} = vee(M, p, X - X̂) return Xc end -# function (cf::CalcFactor{<:Pose3Pose3})(X, p, q) -# M = cf.manifold # getManifold(Pose3) -# ϵX = exp(M, getPointIdentity(M), X) -# q̂ = ArrayPartition(p.x[2]*ϵX.x[1] + p.x[1], p.x[2]*ϵX.x[2]) -# Xc = vee(M, q, log(M, q, q̂)) -# return Xc -# end - ## #TODO is this manifold not SO3 DFG.@defObservationType Pose3Pose3RotOffset AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) From 1476fbbccbd184076d2f6b54c4b542e724acc788 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Mon, 11 Aug 2025 19:20:58 +0200 Subject: [PATCH 13/18] manual merge poses.jl and more lie updates --- RoMETypes/src/variables/VariableTypes.jl | 38 ++++------- src/Deprecated.jl | 38 +++++------ src/canonical/GenerateCommon.jl | 4 +- src/factors/Bearing2D.jl | 4 +- src/factors/BearingRange2D.jl | 2 +- src/factors/DynPose2D.jl | 4 +- src/factors/MutablePose2Pose2.jl | 2 +- src/factors/PartialPose3.jl | 14 ++-- src/factors/Poses.jl | 46 ++++---------- src/manifolds/SOnxRn_MetricManifold.jl | 81 +++--------------------- src/services/g2oParser.jl | 2 +- 11 files changed, 73 insertions(+), 162 deletions(-) diff --git a/RoMETypes/src/variables/VariableTypes.jl b/RoMETypes/src/variables/VariableTypes.jl index e58482b7..3ae6f095 100644 --- a/RoMETypes/src/variables/VariableTypes.jl +++ b/RoMETypes/src/variables/VariableTypes.jl @@ -25,7 +25,11 @@ $(TYPEDEF) Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM. """ -@defVariable Pose2 SOnxRn_MetricManifold(2) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +@defVariable( + Pose2, + TranslationGroup(2) × SpecialOrthogonalGroup(2), #TODO look at using SOnxRn_MetricManifold(2) + ArrayPartition(SA[0;0.0], SA[1 0; 0 1.0]) +) """ $(TYPEDEF) @@ -37,22 +41,17 @@ Future: - Work in progress on AMP3D for proper non-Euler angle on-manifold operations. - TODO the AMP upgrade is aimed at resolving 3D to Quat/SE3/SP3 -- current Euler angles will be replaced """ -@defVariable Pose3 SpecialEuclidean(3; vectors=HybridTangentRepresentation()) ArrayPartition(SA[0;0;0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) - - -@defVariable Rotation3 SpecialOrthogonal(3) SA[1 0 0; 0 1 0; 0 0 1.0] +@defVariable( + Pose3, + TranslationGroup(3) × SpecialOrthogonalGroup(3), #TODO look at using SOnxRn_MetricManifold(3) + ArrayPartition(SA[0;0;0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) +) +@defVariable Rotation3 SpecialOrthogonalGroup(3) SA[1 0 0; 0 1 0; 0 0 1.0] @defVariable( RotVelPos, - Manifolds.ProductGroup( - ProductManifold( - SpecialOrthogonal(3), - TranslationGroup(3), - TranslationGroup(3) - ), - LeftInvariantRepresentation() - ), + SpecialOrthogonalGroup(3) × TranslationGroup(3) × TranslationGroup(3), ArrayPartition( SA[1 0 0; 0 1 0; 0 0 1.0], SA[0; 0; 0.0], @@ -64,13 +63,7 @@ Future: # 3 translations and 3 velocity in graph-base-frame @defVariable( VelPos3, - Manifolds.ProductGroup( - ProductManifold( - TranslationGroup(3), - TranslationGroup(3) - ), - LeftInvariantRepresentation() - ), + TranslationGroup(3) × TranslationGroup(3), ArrayPartition( SA[0; 0; 0.0], SA[0; 0; 0.0] @@ -101,10 +94,7 @@ Note """ @defVariable( DynPose2, - Manifolds.ProductGroup( - ProductManifold(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), TranslationGroup(2)), - LeftInvariantRepresentation() - ), + TranslationGroup(3) × SpecialOrthogonalGroup(3) × TranslationGroup(2), #FIXME SOnxRn(2) or SE(2) ArrayPartition(ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]),SA[0;0.0]) ) diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 1daae1f0..4c964e7e 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -4,14 +4,14 @@ ##============================================================================== -Base.convert( - ::Type{<:Tuple}, - ::IIF.InstanceType{typeof(getManifold(RotVelPos))} -) = ( - :Circular,:Circular,:Circular, - :Euclid,:Euclid,:Euclid, - :Euclid,:Euclid,:Euclid -) +# Base.convert( +# ::Type{<:Tuple}, +# ::IIF.InstanceType{typeof(getManifold(RotVelPos))} +# ) = ( +# :Circular,:Circular,:Circular, +# :Euclid,:Euclid,:Euclid, +# :Euclid,:Euclid,:Euclid +# ) ##============================================================================== @@ -61,16 +61,16 @@ Base.convert( # legacy support, will be deprecated -Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Circular) -Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid) -Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(BearingRange_Manifold)}) = (:Circular,:Euclid) -Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(getManifold(DynPose2))}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid) +# Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(AMP.SE2_Manifold)}) = (:Euclid, :Euclid, :Circular) +# Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(SE2E2_Manifold)}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid) +# Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(BearingRange_Manifold)}) = (:Circular,:Euclid) +# Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(getManifold(DynPose2))}) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid) -Base.convert( - ::Type{<:Tuple}, - ::IIF.InstanceType{typeof(Manifolds.ProductGroup(ProductManifold(SpecialEuclidean(2), TranslationGroup(2)), LeftInvariantRepresentation()))} -) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid) +# Base.convert( +# ::Type{<:Tuple}, +# ::IIF.InstanceType{typeof(Manifolds.ProductGroup(ProductManifold(SpecialEuclideanGroup(2), TranslationGroup(2)), LeftInvariantRepresentation()))} +# ) = (:Euclid,:Euclid,:Circular,:Euclid,:Euclid) # Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Point2Point2}) = AMP.Euclid2 @@ -81,9 +81,9 @@ Base.convert( # 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}) = begin @warn("FIXME"); SE2E2_Manifold end -Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{VelPose2VelPose2}) = begin @warn("FIXME"); SE2E2_Manifold end +# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{DynPoint2DynPoint2}) = AMP.Euclid4 +# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{DynPose2DynPose2}) = begin @warn("FIXME"); SE2E2_Manifold end +# Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{VelPose2VelPose2}) = begin @warn("FIXME"); SE2E2_Manifold end diff --git a/src/canonical/GenerateCommon.jl b/src/canonical/GenerateCommon.jl index b8499aa2..4981caac 100644 --- a/src/canonical/GenerateCommon.jl +++ b/src/canonical/GenerateCommon.jl @@ -230,11 +230,11 @@ function generateField_InertialMeasurement(; accels = Vector{typeof(accel0)}() push!(accels, deepcopy(accel0) + an()) # accels = [deepcopy(accel0) + an()] - M = SpecialOrthogonal(3) + M = SpecialOrthogonalGroup(3) # b_a = [0.1, 0, 0] for g in gyros[1:end-1] - X = hat(M, Identity(M), g) + X = hat(LieAlgebra(M), g) exp!(M, w_R_b, w_R_b, X*dt) push!(accels, (b_a .+ an()) + w_R_b' * accel0) end diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl index fbccd296..9cb50bb2 100644 --- a/src/factors/Bearing2D.jl +++ b/src/factors/Bearing2D.jl @@ -15,9 +15,9 @@ Base.@kwdef struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IIF.AbstractM Z::B = Normal() end -preambleCache(::AbstractDFG, ::AbstractVector{<:DFGVariable}, ::Pose2Point2Bearing) = P2P2BearingReuse() +preambleCache(::AbstractDFG, ::AbstractVector{<:VariableCompute}, ::Pose2Point2Bearing) = P2P2BearingReuse() -getManifold(::Pose2Point2Bearing) = SpecialOrthogonal(2) +getManifold(::Pose2Point2Bearing) = SpecialOrthogonalGroup(2) # Pose2Point2Bearing(x1::B) where {B <: IIF.SamplableBelief} = Pose2Point2Bearing{B}(x1) function (cfo::CalcFactor{<:Pose2Point2Bearing})(X, p, l) diff --git a/src/factors/BearingRange2D.jl b/src/factors/BearingRange2D.jl index 02e26f66..c74fa501 100644 --- a/src/factors/BearingRange2D.jl +++ b/src/factors/BearingRange2D.jl @@ -12,7 +12,7 @@ mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.Sampla range::R end -getManifold(::IIF.InstanceType{<:Pose2Point2BearingRange}) = ProductGroup(ProductManifold(SpecialOrthogonal(2), TranslationGroup(1)), LeftInvariantRepresentation()) +getManifold(::IIF.InstanceType{<:Pose2Point2BearingRange}) = SpecialOrthogonalGroup(2) × TranslationGroup(1) function getSample(cfo::CalcFactor{<:Pose2Point2BearingRange}) # defaults, TODO better reuse diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl index 9f8777f3..2ff2940c 100644 --- a/src/factors/DynPose2D.jl +++ b/src/factors/DynPose2D.jl @@ -21,7 +21,7 @@ function getSample(cf::CalcFactor{<:DynPose2VelocityPrior}) Xc = [rand(Zpose);rand(Zvel)] # X = get_vector.(Ref(M), Ref(p), Xc, Ref(DefaultOrthogonalBasis())) - X = hat(M, p, Xc) + X = hat(LieAlgebra(M), Xc) points = exp(M, p, X) return points @@ -144,7 +144,7 @@ $(TYPEDEF) Base.@kwdef struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeMinimize Z::T = MvNormal(zeros(5), diagm([0.01;0.01;0.001;0.1;0.1].^2)) end -preambleCache(::AbstractDFG, ::AbstractVector{<:DFGVariable}, ::DynPose2DynPose2) = zeros(5) +preambleCache(::AbstractDFG, ::AbstractVector{<:VariableCompute}, ::DynPose2DynPose2) = zeros(5) # FIXME ON FIRE, must update to new Manifolds style factors getManifold(::DynPose2DynPose2) = getManifold(DynPose2) diff --git a/src/factors/MutablePose2Pose2.jl b/src/factors/MutablePose2Pose2.jl index abc037cd..f438f040 100644 --- a/src/factors/MutablePose2Pose2.jl +++ b/src/factors/MutablePose2Pose2.jl @@ -28,7 +28,7 @@ Pose2Pose2, Pose3Pose3, InertialPose3, DynPose2Pose2, Point2Point2, VelPoint2Vel """ function (cf::CalcFactor{<:MutablePose2Pose2Gaussian})(X, p, q) M = getManifold(Pose2) - q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups + q̂ = LieGroups.compose(M, p, exp(M, X)) #for groups #TODO allocalte for vee! see Manifolds #412, fix for AD Xc = zeros(3) vee!(M, Xc, q, log(M, q, q̂)) diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index 2c5855d4..ee20a78b 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -207,19 +207,19 @@ struct Pose3Pose3Rotation{T <: SamplableBelief} <: IIF.AbstractManifoldMinimize end Pose3Pose3Rotation(z::SamplableBelief) = Pose3Pose3Rotation(z, (4,5,6)) -getManifold(::Pose3Pose3Rotation) = SpecialOrthogonal(3) +getManifold(::Pose3Pose3Rotation) = SpecialOrthogonalGroup(3) function (cfo::CalcFactor{<:Pose3Pose3Rotation})(Xm, wTp, wTq ) # - M = SpecialOrthogonal(3) - + G = SpecialOrthogonalGroup(3) + 𝔤 = LieAlgebra(G) p = wTp.x[2] q = wTq.x[2] - X = log(M, p, q) - Xc = vee(M, p, X) - - Xc_m = vee(M, p, Xm) + X = log(G, p, q) + Xc = vee(𝔤, X) + + Xc_m = vee(𝔤, Xm) #TODO Xm - Xc or Xc - Xm? return Xc_m - Xc diff --git a/src/factors/Poses.jl b/src/factors/Poses.jl index cb2e3a3b..e1d050d6 100644 --- a/src/factors/Poses.jl +++ b/src/factors/Poses.jl @@ -1,21 +1,8 @@ -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 - -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} +function (cf::CalcFactor{<:PriorPose2})(m, p) M = getManifold(PriorPose2) - X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? - return vee(M, p, X) + X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? Also update the rest if this is wrong. + return vee(LieAlgebra(M), X) end ## NOTE likely deprecated comparitors, see DFG compareFields, compareAll instead @@ -23,34 +10,29 @@ compare(a::PriorPose2, b::PriorPose2; tol::Float64 = 1e-10) = compareDensity(a.Z function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M - M = getManifold(Pose2) + M = getManifold(Pose2Pose2) X̂ = log(M, p, q) - return vee(M, p, X - X̂) # TODO check sign + return vee(LieAlgebra(M), X - X̂) end # FIXME, rather have separate compareDensity functions compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) -# regular prior for Pose3 - function (cf::CalcFactor{<:PriorPose3})(m, p) - M = getManifold(Pose3) - Xc = vee(M, p, log(M, p, m)) - return Xc + M = getManifold(PriorPose3) + return vee(LieAlgebra(M), log(M, p, m)) end -# Pose3Pose3 evaluation functions - function (cf::CalcFactor{<:Pose3Pose3})(X, p::ArrayPartition{T}, q) where {T} M = getManifold(Pose3Pose3) X̂ = log(M, p, q) - Xc::SVector{6, T} = vee(M, p, X - X̂) + Xc::SVector{6, T} = vee(LieAlgebra(M), X - X̂) return Xc end ## #TODO is this manifold not SO3 -DFG.@defObservationType Pose3Pose3RotOffset AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3RotOffset AbstractManifoldMinimize SOnxRn_MetricManifold(3) # measurement is in frame a, for example imu frame # p and q is in frame b, for example body frame @@ -67,7 +49,7 @@ function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa) end ## -DFG.@defObservationType Pose3Pose3Transform AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3Transform AbstractManifoldMinimize SOnxRn_MetricManifold(3) function (cf::CalcFactor{<:Pose3Pose3Transform})(p_NX, p, q, Δ) M = getManifold(Pose3Pose3Transform) @@ -87,11 +69,11 @@ end $(TYPEDEF) Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation. """ -DFG.@defObservationType Pose3Pose3UnitTrans AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors = HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3UnitTrans AbstractManifoldMinimize SOnxRn_MetricManifold(3) function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where {T} - M = getManifold(Pose3) - q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X)) - Xc::SVector{6, T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis()) + M = getManifold(Pose3Pose3UnitTrans) + q̂ = exp(M, p, X) + Xc::SVector{6, T} = vee(LieAlgebra(M), log(M, q, q̂)) return SVector{6, T}(normalize(Xc[1:3])..., Xc[4:6]...) end diff --git a/src/manifolds/SOnxRn_MetricManifold.jl b/src/manifolds/SOnxRn_MetricManifold.jl index 06b37b9f..e71255ab 100644 --- a/src/manifolds/SOnxRn_MetricManifold.jl +++ b/src/manifolds/SOnxRn_MetricManifold.jl @@ -1,4 +1,6 @@ +using Manifolds: RiemannianMetric, MetricManifold, AbstractBasis +using ManifoldsBase: submanifold_components # Left Invariant Rigid Body Kinematics Metric CrokeKumar eq 61. # A family of left invariant metrics: @@ -84,78 +86,15 @@ Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition( Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) #FIXME remove, only temporary workaround as first signiture is used in many places -@deprecate LieGroups.identity_element(G::LieGroup, p) identity_element(G, typeof(p)) false +# @deprecate LieGroups.identity_element(G::AbstractLieGroup, p) identity_element(G, typeof(p)) false # FIXME why is this still needed, hopefully can be removed soon 🐛💥 -Base.convert(::Type{<:Tuple}, ::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) +# Base.convert(::Type{<:Tuple}, ::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) +AMP._manifoldtuple(::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) +AMP._manifoldtuple(::typeof(SOnxRn_MetricManifold(3))) = (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular) +#FIXME +DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) -## ======================================================================================= -## SE2 + metric -## TODO move to test file -if false -M = SOnxRn_MetricManifold(2) -G = base_manifold(M) -ε = identity_element(M) -T = typeof(identity_element(M)) -Xⁱ = [10, 1, pi/4] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) -q = compose(G, p, exp(base_manifold(G), ε, X)) - -q ≈ exp(M, p, X) -X ≈ log(M, p, q) - - -Xⁱ = [1, 1, 1] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) - -W = diagm([1,1,2]) - -XX = hat(LieAlgebra(G), Xⁱ) -0.5*tr(XX*W*XX') -inner(M, p, X, X) -inner(base_manifold(G), p, X, X) - -Manifolds.distance(M, ε, p) -Manifolds.distance(base_manifold(G), ε, p) - -## SE3 + metric - -M = SOnxRn_MetricManifold(3) -G = base_manifold(M) -ε = identity_element(M) -T = typeof(identity_element(M)) -Xⁱ = [10, 1, 0, 0, 0, pi/4] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) -q = compose(G, p, exp(base_manifold(G), ε, X)) - -q ≈ exp(M, p, X) -X ≈ log(M, p, q) - - -Xⁱ = [0, 0, 0, 0, 1, 1] -X = hat(LieAlgebra(G), Xⁱ, T) -p = exp(base_manifold(G), ε, X) - -0.5*tr(X.x[2]*X.x[2]') -dot(X.x[2], X.x[2]) - -inner(M, p, X, X) -inner(base_manifold(G), p, X, X) - -Manifolds.distance(M, ε, p) -Manifolds.distance(base_manifold(G), ε, p) - -Xⁱ = [1, 1, 0, 0, 1, 1] -W = diagm([1,1,1,2]) -X = hat(LieAlgebra(G), Xⁱ, T) -XX = hat(LieAlgebra(G), Xⁱ) -0.5*tr(XX*W*XX') -inner(M, p, X, X) -inner(base_manifold(G), p, X, X) - - -end \ No newline at end of file +LieGroups.LieAlgebra(G::SOnxRn_MetricManifoldType) = LieAlgebra(base_manifold(G)) \ No newline at end of file diff --git a/src/services/g2oParser.jl b/src/services/g2oParser.jl index a1d66391..33590c30 100644 --- a/src/services/g2oParser.jl +++ b/src/services/g2oParser.jl @@ -124,7 +124,7 @@ function parseG2oInstruction!(fg::AbstractDFG, # MU1 = Unitary(1,ℍ) # ϵU1 = identity_element(MU1) - MSO3 = SpecialOrthogonal(3) + MSO3 = SpecialOrthogonalGroup(3) ϵSO3 = identity_element(MSO3) # Need to add a relative pose measurement between two variables. From 1812bbcc62b5079ad7adb51edf45daca6bde44fc Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 15 Aug 2025 21:24:27 +0200 Subject: [PATCH 14/18] test passed locally --- RoMETypes/Project.toml | 6 + RoMETypes/src/RoMETypes.jl | 12 +- RoMETypes/src/factors/FactorTypes.jl | 16 +- .../src/manifolds/SOnxRn_MetricManifold.jl | 87 ++++++++ RoMETypes/src/variables/VariableTypes.jl | 6 +- ext/RoMECameraModelsExt.jl | 18 +- ext/RoMEDiffEqExt.jl | 2 +- ext/factors/InertialDynamic.jl | 15 +- src/ExportAPI.jl | 2 +- src/RoME.jl | 15 +- src/factors/Bearing2D.jl | 18 +- src/factors/BearingRange2D.jl | 8 +- src/factors/DynPoint2D.jl | 36 +--- src/factors/DynPose2D.jl | 35 +-- src/factors/Inertial/IMUDeltaFactor.jl | 71 ++++--- src/factors/Inertial/PriorIMUBias.jl | 24 +-- src/factors/MutablePose2Pose2.jl | 2 +- src/factors/PartialPose3.jl | 29 +-- src/factors/PartialPriorPose2.jl | 2 +- src/factors/Polar.jl | 2 +- src/factors/Pose2D.jl | 4 +- src/factors/Pose2Point2.jl | 12 +- src/factors/Poses.jl | 30 ++- src/factors/PriorPose2.jl | 14 +- src/factors/PriorVelPos3.jl | 22 +- src/factors/Range2D.jl | 26 +-- src/factors/SensorModels.jl | 2 +- src/factors/VelAlign.jl | 27 +-- src/factors/VelPoint2D.jl | 21 +- src/factors/VelPosRotVelPos.jl | 27 +-- src/factors/VelPose2D.jl | 33 ++- src/manifolds/SOnxRn_MetricManifold.jl | 100 --------- src/services/AdditionalUtils.jl | 2 +- src/services/FixmeManifolds.jl | 2 +- src/services/ManifoldUtils.jl | 23 +- test/inertial/testIMUDeltaFactor.jl | 52 ++--- test/inertial/testODE_INS.jl | 8 +- test/runtests.jl | 30 +-- test/testBearing2D.jl | 4 +- test/testBearingRange2D.jl | 9 +- test/testDidsonFunctions.jl | 4 +- test/testG2oExportSE3.jl | 3 +- test/testGenericProjection.jl | 5 +- test/testHexagonal2D_CliqByCliq.jl | 2 +- test/testInflation380.jl | 5 +- test/testMultimodalRangeBearing.jl | 9 +- test/testParametric.jl | 14 +- test/testParametricSimulated.jl | 8 +- test/testPartialPose3.jl | 12 +- test/testPose3.jl | 6 +- test/testPose3Pose3NH.jl | 16 +- test/testSEParameterizations.jl | 199 ------------------ test/testhigherdimroots.jl | 3 +- test/testpackingconverters.jl | 136 +++++++++--- test/threeDimLinearProductTest.jl | 67 +++--- 55 files changed, 535 insertions(+), 808 deletions(-) create mode 100644 RoMETypes/src/manifolds/SOnxRn_MetricManifold.jl delete mode 100644 src/manifolds/SOnxRn_MetricManifold.jl delete mode 100644 test/testSEParameterizations.jl diff --git a/RoMETypes/Project.toml b/RoMETypes/Project.toml index 9a577f59..40b99f5c 100644 --- a/RoMETypes/Project.toml +++ b/RoMETypes/Project.toml @@ -6,13 +6,19 @@ version = "0.1.0" [deps] DistributedFactorGraphs = "b5cc3c7e-6572-11e9-2517-99fb8daf2f04" DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" +LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f" +LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" +ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb" RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" [compat] DistributedFactorGraphs = "0.27.0" DocStringExtensions = "0.9.3" +LieGroups = "0.1.3" +LinearAlgebra = "1.11.0" Manifolds = "0.10" +ManifoldsBase = "1.2.0" RecursiveArrayTools = "3.27.0" StaticArrays = "1.9.7" diff --git a/RoMETypes/src/RoMETypes.jl b/RoMETypes/src/RoMETypes.jl index 08cc8961..7b08fecd 100644 --- a/RoMETypes/src/RoMETypes.jl +++ b/RoMETypes/src/RoMETypes.jl @@ -2,11 +2,16 @@ module RoMETypes using DistributedFactorGraphs using DocStringExtensions -using Manifolds +using LieGroups using RecursiveArrayTools using StaticArrays +using LinearAlgebra -import DistributedFactorGraphs: getVariableType, AbstractManifoldMinimize +using ManifoldsBase: submanifold_components, TangentSpaceType, AbstractBasis, RiemannianMetric +using Manifolds: MetricManifold + +import ManifoldsBase +import Manifolds export Point2, @@ -38,6 +43,9 @@ export PriorPose2, PackedPriorPose2 +export SOnxRn_MetricManifold + +include("manifolds/SOnxRn_MetricManifold.jl") include("variables/VariableTypes.jl") include("factors/FactorTypes.jl") diff --git a/RoMETypes/src/factors/FactorTypes.jl b/RoMETypes/src/factors/FactorTypes.jl index 5c72724f..36fb3ea2 100644 --- a/RoMETypes/src/factors/FactorTypes.jl +++ b/RoMETypes/src/factors/FactorTypes.jl @@ -7,12 +7,12 @@ $(TYPEDEF) Direction observation information of a `Point3` variable. """ -DFG.@defObservationType PriorPoint3 DFG.AbstractPrior Manifolds.TranslationGroup(3) -DFG.@defObservationType Point3Point3 AbstractManifoldMinimize Manifolds.TranslationGroup(3) +DFG.@defObservationType PriorPoint3 PriorObservation LieGroups.TranslationGroup(3) +DFG.@defObservationType Point3Point3 RelativeObservation LieGroups.TranslationGroup(3) -DFG.@defObservationType Point2Point2 AbstractManifoldMinimize Manifolds.TranslationGroup(2) -DFG.@defObservationType PriorPoint2 DFG.AbstractPrior Manifolds.TranslationGroup(2) +DFG.@defObservationType Point2Point2 RelativeObservation LieGroups.TranslationGroup(2) +DFG.@defObservationType PriorPoint2 PriorObservation LieGroups.TranslationGroup(2) # ======================================================================================== # Pose @@ -45,7 +45,7 @@ Related [`Pose3Pose3`](@ref), [`Point2Point2`](@ref), [`MutablePose2Pose2Gaussian`](@ref), [`DynPose2`](@ref), [`IMUDeltaFactor`](@ref) """ -DFG.@defObservationType Pose2Pose2 AbstractManifoldMinimize Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +DFG.@defObservationType Pose2Pose2 RelativeObservation SOnxRn_MetricManifold(2) """ $(TYPEDEF) @@ -58,7 +58,7 @@ Example: PriorPose2( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) ) ``` """ -DFG.@defObservationType PriorPose2 AbstractPrior Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +DFG.@defObservationType PriorPose2 PriorObservation TranslationGroup(2) × SpecialOrthogonalGroup(2) -DFG.@defObservationType Pose3Pose3 AbstractManifoldMinimize Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) -DFG.@defObservationType PriorPose3 AbstractPrior Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +DFG.@defObservationType Pose3Pose3 RelativeObservation SOnxRn_MetricManifold(3) +DFG.@defObservationType PriorPose3 PriorObservation TranslationGroup(3) × SpecialOrthogonalGroup(3) diff --git a/RoMETypes/src/manifolds/SOnxRn_MetricManifold.jl b/RoMETypes/src/manifolds/SOnxRn_MetricManifold.jl new file mode 100644 index 00000000..9600d4b5 --- /dev/null +++ b/RoMETypes/src/manifolds/SOnxRn_MetricManifold.jl @@ -0,0 +1,87 @@ +struct MetricLieGroup{ + 𝔽, + O <: AbstractGroupOperation, + M <: ManifoldsBase.AbstractManifold{𝔽}, + L <: LieGroup{𝔽, O, M}, + G <: RiemannianMetric, +} <: AbstractLieGroup{𝔽, O, M} + lie_group::L + metric::G +end + +ManifoldsBase.base_manifold(G::MetricLieGroup) = G.lie_group +ManifoldsBase.submanifold_component(G::MetricLieGroup, args...) = submanifold_component(G.lie_group, args...) +ManifoldsBase.submanifold_components(G::MetricLieGroup, args...) = submanifold_components(G.lie_group, args...) +LieGroups.LieAlgebra(G::MetricLieGroup) = LieAlgebra(base_manifold(G)) +LieGroups.inv!(G::MetricLieGroup, args...) = inv!(base_manifold(G), args...) +LieGroups.inv(G::MetricLieGroup, args...) = inv(base_manifold(G), args...) +LieGroups.compose!(G::MetricLieGroup, args...) = compose!(base_manifold(G), args...) +LieGroups.compose(G::MetricLieGroup, args...) = compose(base_manifold(G), args...) +LieGroups.identity_element(G::MetricLieGroup, args...) = identity_element(base_manifold(G), args...) +LieGroups.identity_element!(G::MetricLieGroup, args...) = identity_element!(base_manifold(G), args...) + + +# Left Invariant Rigid Body Kinematics Metric CrokeKumar eq 61. +# A family of left invariant metrics: +# G = [αI 0; 0 βI] +# where α and β are arbitrary constants, satisfies all the equations (80). +# This are the only left-invariant metrics which are compatible with the acceleration connection. +# Can we add α and β as parameters to the metric? +struct LeftInvariantKinematicMetric <: RiemannianMetric end + +function SOnxRn_MetricManifold(n) + MetricLieGroup( + SpecialEuclideanGroup(n; variant = :right), + LeftInvariantKinematicMetric(), + ) +end + +SOnxRn_MetricManifoldType = + Union{typeof(SOnxRn_MetricManifold(2)), typeof(SOnxRn_MetricManifold(3))} + +# geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3 +function Manifolds.exp(M::SOnxRn_MetricManifoldType, X) + G = base_manifold(M) + ε = identity_element(M, typeof(X)) + return exp(base_manifold(G), ε, X) +end + +function Manifolds.exp!(M::SOnxRn_MetricManifoldType, g, X) + G = base_manifold(M) + ε = identity_element(M, typeof(g)) + return exp!(base_manifold(G), g, ε, X) +end + +function ManifoldsBase.log(M::SOnxRn_MetricManifoldType, p) + G = base_manifold(M) + # ε = identity_element(G, typeof(p)) + # X = log(base_manifold(G), ε, p) + PG = ProductLieGroup(map(LieGroup, G.manifold.manifolds, G.op.operations)...) + X = log(PG, p) + return X +end +function Manifolds.log!(M::SOnxRn_MetricManifoldType, X, p) + G = base_manifold(M) + ε = identity_element(M, typeof(p)) + log!(base_manifold(G), X, ε, p) + return X +end + +function Manifolds.inner(M::SOnxRn_MetricManifoldType, p, X, Y) + Xtr = submanifold_components(M, X)[1] + XRo = submanifold_components(M, X)[2] + Ytr = submanifold_components(M, Y)[1] + YRo = submanifold_components(M, Y)[2] + # Metric on Chirikjian, p35 W = diagm([1,1,2]) + return dot(Xtr, Ytr) + dot(XRo, YRo) / 2 +end + + +function DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(2))) + ArrayPartition(SA[0; 0.0], SA[1 0; 0 1.0]) +end +function DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(3))) + ArrayPartition(SA[0, 0, 0.0], SA[1 0 0; 0 1 0; 0 0 1.0]) +end + +# LieGroups.LieAlgebra(G::SOnxRn_MetricManifoldType) = LieAlgebra(base_manifold(G)) \ No newline at end of file diff --git a/RoMETypes/src/variables/VariableTypes.jl b/RoMETypes/src/variables/VariableTypes.jl index 3ae6f095..52fa0953 100644 --- a/RoMETypes/src/variables/VariableTypes.jl +++ b/RoMETypes/src/variables/VariableTypes.jl @@ -94,8 +94,10 @@ Note """ @defVariable( DynPose2, - TranslationGroup(3) × SpecialOrthogonalGroup(3) × TranslationGroup(2), #FIXME SOnxRn(2) or SE(2) - ArrayPartition(ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]),SA[0;0.0]) + # SOnxRn_MetricManifold(2) × TranslationGroup(2), #FIXME SOnxRn(2) or SE(2) + # ArrayPartition(ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]),SA[0;0.0]) + TranslationGroup(2) × SpecialOrthogonalGroup(2) × TranslationGroup(2), + ArrayPartition(SA[0;0.0], SA[1 0; 0 1.0], SA[0;0.0]) ) diff --git a/ext/RoMECameraModelsExt.jl b/ext/RoMECameraModelsExt.jl index e7001a56..89567cc7 100644 --- a/ext/RoMECameraModelsExt.jl +++ b/ext/RoMECameraModelsExt.jl @@ -4,7 +4,7 @@ module RoMECameraModelsExt using CameraModels using StaticArrays -using Manifolds +using LieGroups using DocStringExtensions using Optim using RecursiveArrayTools: ArrayPartition @@ -37,10 +37,10 @@ function (cf::CalcFactor{<:GenericProjection{S,T}})( ) where {S,T} κ = 0.001 - M = SpecialEuclidean(3) + M = SpecialEuclideanGroup(3) w_Ph = SVector{4,Float64}(w_P_o..., 1.0) - c_H_w = inv(affine_matrix(M, w_P_c)) + c_H_w = inv(M, convert(AbstractMatrix, SpecialEuclideanProductPoint(w_P_c))) # predicted projection # c_Xhat = project(S, T, w_P_c, w_P_o) @@ -82,7 +82,7 @@ function solveMultiviewLandmark!( _undistort::Function = (c,p) -> CameraModels.undistortPoint(cam,p) ) # assume to find - M = SpecialEuclidean(3) + M = SpecialEuclideanGroup(3) flbs = ls(dfg, lmlb) # fcs = getFactor.(dfg, flbs) # fcd = getFactorType.(fcs) @@ -93,7 +93,8 @@ function solveMultiviewLandmark!( end function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ=1000) - pred = projectPointFrom(cam, inv(affine_matrix(M,w_T_c)), w_Ph) + c_H_w = inv(M, convert(AbstractMatrix, SpecialEuclideanProductPoint(w_T_c))) + pred = projectPointFrom(cam, c_H_w, w_Ph) # experimental cost function to try force bad reprojects in front of the camera during optimization κ*(abs(pred.depth) - pred.depth)^2 + (meas[1]-pred[1])^2 + (meas[2]-pred[2])^2 end @@ -149,13 +150,14 @@ function solveMultiviewLandmark!( # check depth okay w_Ph = [w_P3...; 1.0] depthok = true - for w_T_c in _w_P_ci - pred = projectPointFrom(cam, inv(affine_matrix(M, w_T_c)), w_Ph) + for w_T_c in _w_P_ci + c_H_w = inv(M, convert(AbstractMatrix, SpecialEuclideanProductPoint(w_T_c))) + pred = projectPointFrom(cam, c_H_w, w_Ph) if pred.depth < 0 depthok = false end end - if w_Res.ls_success && depthok + if Optim.converged(w_Res) && depthok break end if retry <= i diff --git a/ext/RoMEDiffEqExt.jl b/ext/RoMEDiffEqExt.jl index 71c34fa1..c6f0d11e 100644 --- a/ext/RoMEDiffEqExt.jl +++ b/ext/RoMEDiffEqExt.jl @@ -35,7 +35,7 @@ function InertialDynamic( bproblem = problemType(imuKinematic!, state1, (tspan[2], tspan[1]), data; dt = -dt) # build the IIF recognizable object - return DERelative(domain, fproblem, bproblem, data) + return DERelative(domain, fproblem, bproblem, data, nothing) end # function InertialDynamic( diff --git a/ext/factors/InertialDynamic.jl b/ext/factors/InertialDynamic.jl index c9219e8e..d039dd1f 100644 --- a/ext/factors/InertialDynamic.jl +++ b/ext/factors/InertialDynamic.jl @@ -1,19 +1,10 @@ - -@kwdef struct InertialDynamic{D<:SamplableBelief} <: AbstractManifoldMinimize - Z::D -end - -getManifold(::InertialDynamic) = getManifold(RotVelPos) - - - - +DFG.@defObservationType InertialDynamic RelativeObservation getManifold(RotVelPos) ## TODO consolidate inside module as RoME.imuKinematic ## du = f(u, params, t) # then solve ODE function imuKinematic!(du, u, p, t; g=SA[0; 0; 9.81]) # p is IMU input (assumed [.gyro; .accel]) - M = SpecialOrthogonal(3) + M = SpecialOrthogonalGroup(3) R = u.x[1] # i_R_b = w_R_b Rotation V = u.x[2] # Velocity @@ -22,7 +13,7 @@ function imuKinematic!(du, u, p, t; g=SA[0; 0; 9.81]) # A_b = u.x[5] # Accelerometer bias ω_m = p[].gyro(t) - Ω = hat(M, Identity(M), ω_m) # + ω_b) # b_Ωbi skew symmetric (Lie algebra element) + Ω = hat(LieAlgebra(M), ω_m) # + ω_b) # b_Ωbi skew symmetric (Lie algebra element) Ṙ = R * Ω # w_Ṙ_b = i_Ṙ_b = d/dt R = d/dt exp(Ω*Δt) => Ṙ = exp(ΩΔt)*d(ΩΔt)/dt = exp(ΩΔt)*Ω A_m = p[].accel(t) # b_Abi diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl index 00fcf84b..c11e22e9 100644 --- a/src/ExportAPI.jl +++ b/src/ExportAPI.jl @@ -8,7 +8,7 @@ export DFG # from Manifolds -export SpecialEuclidean +export SpecialEuclideanGroup export diff --git a/src/RoME.jl b/src/RoME.jl index a8fa0a0f..8ae26c82 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -21,7 +21,7 @@ using DistributedFactorGraphs, TensorCast, ManifoldsBase, - Manifolds, + # Manifolds, LieGroups using StaticArrays @@ -29,20 +29,22 @@ using PrecompileTools using RecursiveArrayTools # to avoid name conflicts import Manifolds -using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, SpecialOrthogonal, TranslationGroup, identity_element, submanifold_component, Identity, affine_matrix +# using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, SpecialOrthogonal, TranslationGroup, identity_element, submanifold_component, Identity, affine_matrix -import Manifolds: project, project!, identity_element +using ManifoldsBase: check_point, submanifold_component, submanifold_components +using Manifolds: SymmetricPositiveDefinite +# import Manifolds: project, project!, identity_element import Rotations as _Rot # import Rotations: ⊕, ⊖ # TODO deprecate -export SpecialOrthogonal, SpecialEuclidean +export SpecialOrthogonalGroup, SpecialEuclidean export submanifold_component # using Graphs, # TODO determine how many parts still require Graphs still directly import Base: +, \, convert -import TransformUtils: ⊖, ⊕, convert, ominus, veeQuaternion +import TransformUtils: ⊕, convert, ominus, veeQuaternion import IncrementalInference: MB import IncrementalInference: convert, getSample, reshapeVec2Mat, DFG import IncrementalInference: getMeasurementParametric @@ -68,9 +70,6 @@ include("entities/SpecialDefinitions.jl") #uses DFG v0.10.2 @defVariable for above include("services/FixmeManifolds.jl") -include("manifolds/SOnxRn_MetricManifold.jl") - -include("variables/VariableTypes.jl") ## More factor types # RoME internal factors (FYI outside factors are easy, see Caesar documentation) diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl index 9cb50bb2..646effe7 100644 --- a/src/factors/Bearing2D.jl +++ b/src/factors/Bearing2D.jl @@ -11,14 +11,10 @@ end Single dimension bearing constraint from Pose2 to Point2 variable. """ -Base.@kwdef struct Pose2Point2Bearing{B <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize - Z::B = Normal() -end +DFG.@defObservationType Pose2Point2Bearing RelativeObservation SpecialOrthogonalGroup(2) preambleCache(::AbstractDFG, ::AbstractVector{<:VariableCompute}, ::Pose2Point2Bearing) = P2P2BearingReuse() -getManifold(::Pose2Point2Bearing) = SpecialOrthogonalGroup(2) -# Pose2Point2Bearing(x1::B) where {B <: IIF.SamplableBelief} = Pose2Point2Bearing{B}(x1) function (cfo::CalcFactor{<:Pose2Point2Bearing})(X, p, l) # wl = l @@ -31,15 +27,3 @@ function (cfo::CalcFactor{<:Pose2Point2Bearing})(X, p, l) return [δθ] end - -# Packing and Unpacking -Base.@kwdef struct PackedPose2Point2Bearing <: AbstractPackedFactor - Z::PackedSamplableBelief - # _type::String = "RoME.PackedPose2Point2Bearing" -end -function convert(::Type{PackedPose2Point2Bearing}, d::Pose2Point2Bearing{B}) where {B <: IIF.SamplableBelief} - return PackedPose2Point2Bearing(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{Pose2Point2Bearing}, d::PackedPose2Point2Bearing) - Pose2Point2Bearing(convert(SamplableBelief, d.Z)) -end diff --git a/src/factors/BearingRange2D.jl b/src/factors/BearingRange2D.jl index c74fa501..96b7febf 100644 --- a/src/factors/BearingRange2D.jl +++ b/src/factors/BearingRange2D.jl @@ -12,7 +12,7 @@ mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.Sampla range::R end -getManifold(::IIF.InstanceType{<:Pose2Point2BearingRange}) = SpecialOrthogonalGroup(2) × TranslationGroup(1) +DFG.getManifold(::IIF.InstanceType{<:Pose2Point2BearingRange}) = SpecialOrthogonalGroup(2) × TranslationGroup(1) function getSample(cfo::CalcFactor{<:Pose2Point2BearingRange}) # defaults, TODO better reuse @@ -78,10 +78,10 @@ Base.@kwdef struct PackedPose2Point2BearingRange <: AbstractPackedFactor rangstr::PackedSamplableBelief end -function convert( ::Type{<:PackedPose2Point2BearingRange}, d::Pose2Point2BearingRange ) +function DFG.pack(d::Pose2Point2BearingRange ) return PackedPose2Point2BearingRange( convert(PackedSamplableBelief, d.bearing), convert(PackedSamplableBelief, d.range) ) end -function convert( ::Type{<:Pose2Point2BearingRange}, d::PackedPose2Point2BearingRange ) - Pose2Point2BearingRange( convert(SamplableBelief, d.bearstr), convert(SamplableBelief, d.rangstr) ) +function DFG.unpack(d::PackedPose2Point2BearingRange ) + return Pose2Point2BearingRange( convert(SamplableBelief, d.bearstr), convert(SamplableBelief, d.rangstr) ) end diff --git a/src/factors/DynPoint2D.jl b/src/factors/DynPoint2D.jl index 40679c74..ff76f79c 100644 --- a/src/factors/DynPoint2D.jl +++ b/src/factors/DynPoint2D.jl @@ -4,20 +4,13 @@ """ $(TYPEDEF) """ -mutable struct DynPoint2VelocityPrior{T <: SamplableBelief} <: AbstractPrior - Z::T -end - -getManifold(::DynPoint2VelocityPrior) = TranslationGroup(4) +DFG.@defObservationType DynPoint2VelocityPrior PriorObservation TranslationGroup(4) """ $(TYPEDEF) """ -mutable struct DynPoint2DynPoint2{T <: SamplableBelief} <: AbstractManifoldMinimize #RelativeRoots - Z::T -end +DFG.@defObservationType DynPoint2DynPoint2 RelativeObservation TranslationGroup(4) -getManifold(::DynPoint2DynPoint2) = TranslationGroup(4) function (cfo::CalcFactor{<:DynPoint2DynPoint2})(z, xi, xj) @@ -32,11 +25,7 @@ end """ $(TYPEDEF) """ -mutable struct Point2Point2Velocity{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize # RelativeMinimize - Z::T -end - -getManifold(::Point2Point2Velocity) = TranslationGroup(4) +DFG.@defObservationType Point2Point2Velocity RelativeObservation TranslationGroup(4) function (cfo::CalcFactor{<:Point2Point2Velocity})( z, xi, @@ -54,24 +43,5 @@ end -## Packing Types================================================================ - - -""" -$(TYPEDEF) -""" -Base.@kwdef struct PackedDynPoint2VelocityPrior <: AbstractPackedFactor - str::PackedSamplableBelief -end - -function convert(::Type{PackedDynPoint2VelocityPrior}, d::DynPoint2VelocityPrior) - return PackedDynPoint2VelocityPrior(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{DynPoint2VelocityPrior}, d::PackedDynPoint2VelocityPrior) - distr = convert(SamplableBelief, d.str) - return DynPoint2VelocityPrior(distr) -end - - # diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl index 2ff2940c..addd86f6 100644 --- a/src/factors/DynPose2D.jl +++ b/src/factors/DynPose2D.jl @@ -10,19 +10,16 @@ mutable struct DynPose2VelocityPrior{T1,T2} <: IncrementalInference.AbstractPrio end DynPose2VelocityPrior(z1::T1,z2::T2) where {T1 <: IIF.SamplableBelief, T2 <: IIF.SamplableBelief} = DynPose2VelocityPrior{T1,T2}(z1,z2) -DFG.getManifold(::DynPose2VelocityPrior) = getManifold(DynPose2) +DFG.getManifold(::Type{<:DynPose2VelocityPrior}) = getManifold(DynPose2) function getSample(cf::CalcFactor{<:DynPose2VelocityPrior}) Zpose = cf.factor.Zpose Zvel = cf.factor.Zvel - p = getPointIdentity(DynPose2()) M = getManifold(cf) - Xc = [rand(Zpose);rand(Zvel)] - # X = get_vector.(Ref(M), Ref(p), Xc, Ref(DefaultOrthogonalBasis())) X = hat(LieAlgebra(M), Xc) - points = exp(M, p, X) + points = exp(M, X) return points end @@ -141,13 +138,12 @@ end """ $(TYPEDEF) """ -Base.@kwdef struct DynPose2DynPose2{T <: IIF.SamplableBelief} <: AbstractRelativeMinimize - Z::T = MvNormal(zeros(5), diagm([0.01;0.01;0.001;0.1;0.1].^2)) -end +DFG.@defObservationType DynPose2DynPose2 RelativeObservation getManifold(DynPose2) + preambleCache(::AbstractDFG, ::AbstractVector{<:VariableCompute}, ::DynPose2DynPose2) = zeros(5) # FIXME ON FIRE, must update to new Manifolds style factors -getManifold(::DynPose2DynPose2) = getManifold(DynPose2) +# DFG.getManifold(::Type{<:DynPose2DynPose2}) = getManifold(DynPose2) # FIXME, should produce tangents, not coordinates. getSample(cf::CalcFactor{<:DynPose2DynPose2}) = rand(cf.factor.Z) function (cf::CalcFactor{<:DynPose2DynPose2})(meas, @@ -179,24 +175,3 @@ function compare(a::DynPose2DynPose2, b::DynPose2DynPose2; tol::Float64=1e-10):: # TP = TP && norm(a.reuseres - b.reuseres) < tol return TP end - - -""" -$(TYPEDEF) -""" -Base.@kwdef struct PackedDynPose2DynPose2 <: AbstractPackedFactor - Z::PackedSamplableBelief -end - -function convert(::Type{PackedDynPose2DynPose2}, d::DynPose2DynPose2) - return PackedDynPose2DynPose2(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{DynPose2DynPose2}, d::PackedDynPose2DynPose2) - return DynPose2DynPose2(convert(SamplableBelief, d.Z)) -end - - - - - -# diff --git a/src/factors/Inertial/IMUDeltaFactor.jl b/src/factors/Inertial/IMUDeltaFactor.jl index 4cc61798..52f4ed69 100644 --- a/src/factors/Inertial/IMUDeltaFactor.jl +++ b/src/factors/Inertial/IMUDeltaFactor.jl @@ -1,6 +1,5 @@ -using Manifolds using StaticArrays -using Rotations +using Rotations: RotationVec using LinearAlgebra using DistributedFactorGraphs using Dates @@ -29,14 +28,18 @@ Affine representation ArrayPartition representation (TODO maybe swop order to [Δp; Δv; ΔR; Δt]) Δ = [ΔR; Δv; Δp; Δt] """ -const SpecialGalileanGroup = GroupManifold{ℝ, SpecialGalileanManifold, MultiplicationOperation} - -SpecialGalileanGroup() = GroupManifold(SpecialGalileanManifold(), MultiplicationOperation(), LeftInvariantRepresentation()) +const SpecialGalileanGroup = LieGroup{ℝ, MatrixMultiplicationGroupOperation, SpecialGalileanManifold}#GroupManifold{ℝ, SpecialGalileanManifold, MultiplicationOperation} +SpecialGalileanGroup() = LieGroup(SpecialGalileanManifold(), MatrixMultiplicationGroupOperation()) Manifolds.manifold_dimension(::SpecialGalileanManifold) = 9 -function Manifolds.identity_element(M::SpecialGalileanGroup) # was #SMatrix{5,5,Float64}(I) +# SGalProductPoint +# SGalMatrixPoint +# SGalProductTangentVector +# SGalMatrixTangentVector + +function LieGroups.identity_element(M::SpecialGalileanGroup) # was #SMatrix{5,5,Float64}(I) ArrayPartition( SMatrix{3,3,Float64}(I), # ΔR @SVector(zeros(3)), # Δv @@ -47,7 +50,7 @@ end DFG.getPointIdentity(M::SpecialGalileanGroup) = identity_element(M) -function Manifolds.affine_matrix(G::SpecialGalileanGroup, p::ArrayPartition{T}) where T<:Real +function affine_matrix(G::SpecialGalileanGroup, p::ArrayPartition{T}) where T<:Real return vcat( hcat(p.x[1], p.x[2], p.x[3]), @SMatrix [0 0 0 1 p.x[4]; @@ -63,7 +66,7 @@ function vector_affine_matrix(G::SpecialGalileanGroup, X::ArrayPartition{T}) whe ) end -function Manifolds.inv(M::SpecialGalileanGroup, p) +function LieGroups.inv(M::SpecialGalileanGroup, p) ΔR = p.x[1] Δv = p.x[2] Δp = p.x[3] @@ -77,7 +80,7 @@ function Manifolds.inv(M::SpecialGalileanGroup, p) ) end -function Manifolds.compose(M::SpecialGalileanGroup, p, q) +function LieGroups.compose(M::SpecialGalileanGroup, p, q) ΔR = p.x[1] Δv = p.x[2] Δp = p.x[3] @@ -96,7 +99,10 @@ function Manifolds.compose(M::SpecialGalileanGroup, p, q) ) end -function Manifolds.hat(M::SpecialGalileanGroup, Xⁱ::SVector{10, T}) where T<:Real +function LieGroups.hat( + M::Union{<:SpecialGalileanGroup, <:typeof(LieAlgebra(SpecialGalileanGroup()))}, + Xⁱ::SVector{10, T} +) where T<:Real return ArrayPartition( ApproxManifoldProducts.skew(Xⁱ[SA[7:9...]]), # θ ωΔt Xⁱ[SA[4:6...]], # ν aΔt @@ -105,7 +111,10 @@ function Manifolds.hat(M::SpecialGalileanGroup, Xⁱ::SVector{10, T}) where T<:R ) end -function Manifolds.vee(M::SpecialGalileanGroup, X::ArrayPartition{T}) where T<:Real +function LieGroups.vee( + M::Union{<:SpecialGalileanGroup, <:typeof(LieAlgebra(SpecialGalileanGroup()))}, + X::ArrayPartition{T} +) where T<:Real return SVector{10,T}( X.x[3]..., # ν aΔt 4:6 X.x[2]..., # ρ vΔt 1:3 @@ -148,9 +157,7 @@ function _P(θ⃗) end end -#TODO rename to exp_lie? -Manifolds.exp(M::SpecialGalileanGroup, X::ArrayPartition{T}) where T<:Real = error("use exp_lie instead") -function Manifolds.exp_lie(M::SpecialGalileanGroup, X::ArrayPartition{T}) where T<:Real +function LieGroups.exp(M::SpecialGalileanGroup, X::ArrayPartition{T}) where T<:Real θ⃗ₓ = X.x[1] # ωΔt ν = X.x[2] # aΔt @@ -164,7 +171,7 @@ function Manifolds.exp_lie(M::SpecialGalileanGroup, X::ArrayPartition{T}) where P = _P(θ⃗) Q = _Q(θ⃗) - M_SO3 = SpecialOrthogonal(3) + M_SO3 = SpecialOrthogonalGroup(3) q = ArrayPartition( exp(M_SO3, getPointIdentity(M_SO3), θ⃗ₓ), Q*ν, @@ -174,14 +181,12 @@ function Manifolds.exp_lie(M::SpecialGalileanGroup, X::ArrayPartition{T}) where return q end -#TODO is this now exp_inv? to fit with Manifold.jl (until LieGroups.jl is done) -function Manifolds.exp(M::SpecialGalileanGroup, p::ArrayPartition{T}, X::ArrayPartition{T}) where T<:Real - q = exp_lie(M, X) - return Manifolds.compose(M, p, q) +function LieGroups.exp(M::SpecialGalileanGroup, p::ArrayPartition{T}, X::ArrayPartition{T}) where T<:Real + q = exp(M, X) + return LieGroups.compose(M, p, q) end -Manifolds.log(M::SpecialGalileanGroup, p::ArrayPartition{T}) where T<:Real = error("use log_lie instead") -function Manifolds.log_lie(M::SpecialGalileanGroup, p) +function LieGroups.log(M::SpecialGalileanGroup, p) ΔR = p.x[1] Δv = p.x[2] Δp = p.x[3] @@ -195,7 +200,7 @@ function Manifolds.log_lie(M::SpecialGalileanGroup, p) iQ = inv(Q) return ArrayPartition( - log_lie(SpecialOrthogonal(3), ΔR), # θ⃗ₓ + log(SpecialOrthogonalGroup(3), ΔR), # θ⃗ₓ iQ*Δv, # ν aΔt iQ*(Δp - P*iQ*Δv*Δt), # ρ vΔt Δt @@ -205,8 +210,8 @@ end #TODO TEST #TODO is this now log_inv? to fit with Manifold.jl (until LieGroups.jl is done) # right-⊖ : Xₚ = q ⊖ p = log(p⁻¹∘q) -function Manifolds.log(M::SpecialGalileanGroup, p, q) - return log_lie(M, Manifolds.compose(M, inv(M, p), q)) +function LieGroups.log(M::SpecialGalileanGroup, p, q) + return log(M, LieGroups.compose(M, inv(M, p), q)) end # compute the expected delta from p to q on the SpecialGalileanGroup @@ -318,7 +323,7 @@ Base.@kwdef struct IMUDeltaFactor{T <: SamplableBelief} <: AbstractManifoldMinim end function IIF.getSample(cf::CalcFactor{<:IMUDeltaFactor}) - return exp_lie(SpecialGalileanGroup(), hat(SpecialGalileanGroup(), SA[rand(cf.factor.Z)..., cf.factor.Δt])) + return exp(SpecialGalileanGroup(), hat(SpecialGalileanGroup(), SA[rand(cf.factor.Z)..., cf.factor.Δt])) end function IIF.getFactorMeasurementParametric(f::IMUDeltaFactor) @@ -326,10 +331,10 @@ function IIF.getFactorMeasurementParametric(f::IMUDeltaFactor) return f.Δ, iΣ end -IIF.getManifold(::IMUDeltaFactor) = SpecialGalileanGroup() +IIF.getManifold(::Type{<:IMUDeltaFactor}) = SpecialGalileanGroup() -function IIF.preambleCache(fg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, ::IMUDeltaFactor) - if vars[1] isa DFGVariable{<:Pose3} +function IIF.preambleCache(fg::AbstractDFG, vars::AbstractVector{<:VariableCompute}, ::IMUDeltaFactor) + if vars[1] isa VariableCompute{<:Pose3} (timestams = (vars[1].nstime, vars[3].nstime),) else (timestams = (vars[1].nstime, vars[2].nstime),) @@ -348,11 +353,11 @@ function (cf::CalcFactor{<:IMUDeltaFactor})( # M = SpecialGalileanGroup() # imu measurment Delta, corrected for bias with # b̄ = cf.factor.b #TODO check if (b - cf.factor.b) is correct - Δi = Manifolds.compose(M, Δmeas, exp_lie(M, hat(M, cf.factor.J_b * (b - cf.factor.b)))) + Δi = LieGroups.compose(M, Δmeas, exp(M, hat(M, cf.factor.J_b * (b - cf.factor.b)))) # expected delta from p to q Δhat = boxminus(M, p, q) # residual - Xhat = log_lie(M, Manifolds.compose(M, inv(M, Δi), Δhat)) + Xhat = log(M, LieGroups.compose(M, inv(M, Δi), Δhat)) Xc_hat = vee(M, Xhat) @assert isapprox(Δi.x[4], Δhat.x[4], atol=1e-6) "Time descrepancy in IMUDeltaFactor: Δt = $(Xc_hat[10]), $(Δi.x[4]), $(Δhat.x[4])" @@ -415,9 +420,9 @@ function integrateIMUDelta(Δij, Σij, Δij_J_b, a, ω, a_b, ω_b, δt, Σy) Xc = SVector{10,Float64}(vcat([0., 0, 0], aδt, ωδt, δt)) X = hat(M, Xc) - δjk = exp_lie(M, X) + δjk = exp(M, X) - Δik = Manifolds.compose(M, Δij, δjk) + Δik = LieGroups.compose(M, Δij, δjk) # Jacobians τ_J_y = _τδt(δt) @@ -469,7 +474,7 @@ function IMUDeltaFactor( Δ, Σ, J_b = preintegrateIMU(accels, gyros, deltatimes, Σy, a_b, ω_b) Δt = Δ.x[4] - Xc = vee(M, log_lie(M, Δ)) + Xc = vee(M, log(M, Δ)) SM = SymmetricPositiveDefinite(9) S = Σ[1:9,1:9] diff --git a/src/factors/Inertial/PriorIMUBias.jl b/src/factors/Inertial/PriorIMUBias.jl index 31bde729..216bc70f 100644 --- a/src/factors/Inertial/PriorIMUBias.jl +++ b/src/factors/Inertial/PriorIMUBias.jl @@ -10,17 +10,7 @@ Example: PriorIMUBias( MvNormal(zeros(6), Matrix(Diagonal(ones(6).^2))) ) ``` """ -Base.@kwdef struct PriorIMUBias{T <: SamplableBelief} <: IncrementalInference.AbstractPrior - Z::T = MvNormal(zeros(6), diagm(0.5.*ones(6))) -end - -DistributedFactorGraphs.getManifold(::InstanceType{PriorIMUBias}) = Manifolds.ProductGroup( - ProductManifold( - TranslationGroup(3), - TranslationGroup(3) - ) -) - +DFG.@defObservationType PriorIMUBias PriorObservation TranslationGroup(3) × TranslationGroup(3) # TODO the log here looks wrong (for gradients), consider: # X = log(p⁻¹ ∘ m) @@ -39,16 +29,4 @@ end #TODO Serialization of reference point p ## Serialization support -""" -$(TYPEDEF) -""" -Base.@kwdef struct PackedPriorIMUBias <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{PackedPriorIMUBias}, d::PriorIMUBias) - return PackedPriorIMUBias(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{PriorIMUBias}, d::PackedPriorIMUBias) - return PriorIMUBias(convert(SamplableBelief, d.Z)) -end diff --git a/src/factors/MutablePose2Pose2.jl b/src/factors/MutablePose2Pose2.jl index f438f040..f6278cc7 100644 --- a/src/factors/MutablePose2Pose2.jl +++ b/src/factors/MutablePose2Pose2.jl @@ -14,7 +14,7 @@ Base.@kwdef mutable struct MutablePose2Pose2Gaussian <: IIF.AbstractManifoldMin end MutablePose2Pose2Gaussian(Z::MvNormal) = MutablePose2Pose2Gaussian(;Z) -DFG.getManifold(::MutablePose2Pose2Gaussian) = getManifold(Pose2) # Manifolds.SpecialEuclidean(2) +DFG.getManifold(::Type{<:MutablePose2Pose2Gaussian}) = getManifold(Pose2) # Manifolds.SpecialEuclidean(2) """ diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index ee20a78b..36e76017 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -17,10 +17,11 @@ end PriorPose3ZRP(z::SamplableBelief,rp::SamplableBelief) = PriorPose3ZRP(;z, rp) # TODO should be dim 3 manifold -getManifold(zrp::PriorPose3ZRP) = ProductManifold(TranslationGroup(1),RealCircleGroup(),RealCircleGroup()) +DFG.getManifold(zrp::Type{<:PriorPose3ZRP}) = TranslationGroup(1) × CircleGroup(ℝ) × CircleGroup(ℝ) # 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 why was the type piracy needed here? +# Manifolds.identity_element(::typeof(ProductManifold(TranslationGroup(1),CircleGroup(ℝ),CircleGroup(ℝ))),dummy=nothing) = ArrayPartition([0.], [0.], [0.]) #FIXME update to also only one measurement function getSample(cf::CalcFactor{<:PriorPose3ZRP}) @@ -37,7 +38,7 @@ function getSample(cf::CalcFactor{<:PriorPose3ZRP}) # 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)) + w_Cp = vee(LieAlgebra(Mf), log(Mf, pt)) return ArrayPartition( [w_Cp[cf.factor.partial[1]]], [w_Cp[cf.factor.partial[2]]], @@ -55,10 +56,10 @@ Base.@kwdef struct PackedPriorPose3ZRP <: AbstractPackedFactor zdata::PackedSamplableBelief rpdata::PackedSamplableBelief end -function convert(::Type{PriorPose3ZRP}, d::PackedPriorPose3ZRP) +function DFG.unpack(d::PackedPriorPose3ZRP) PriorPose3ZRP( convert(SamplableBelief, d.zdata), convert(SamplableBelief, d.rpdata) ) end -function convert(::Type{PackedPriorPose3ZRP}, d::PriorPose3ZRP) +function DFG.pack(d::PriorPose3ZRP) PackedPriorPose3ZRP( convert(PackedSamplableBelief, d.z), convert(PackedSamplableBelief, d.rp) ) end @@ -109,13 +110,13 @@ Pose3Pose3XYYaw(xy::SamplableBelief, yaw::SamplableBelief) = error("Pose3Pose3XY # Pose3Pose3XYYaw(z::SamplableBelief) = Pose3Pose3XYYaw(z, (1,2,4,5,6)) # (1,2,6)) Pose3Pose3XYYaw(z::SamplableBelief) = Pose3Pose3XYYaw(z, (1,2,6)) -getManifold(::Pose3Pose3XYYaw) = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +DFG.getManifold(::Type{<:Pose3Pose3XYYaw}) = SOnxRn_MetricManifold(2) ## NOTE, Yaw only works if you assume a preordained global reference point, such as identity_element(Pose3) function (cfo::CalcFactor{<:Pose3Pose3XYYaw})(X, wTp, wTq ) # - M = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) + M = SOnxRn_MetricManifold(2) rx = normalize(view(wTp.x[2],1:2, 1)) R = SA[rx[1] -rx[2]; @@ -127,10 +128,10 @@ function (cfo::CalcFactor{<:Pose3Pose3XYYaw})(X, wTp, wTq ) rx[2] rx[1]] q = ArrayPartition(view(wTq.x[1], 1:2), R) - q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) + q̂ = exp(M, p, X) #TODO allocalte for vee! see Manifolds #412, fix for AD Xc = zeros(3) - vee!(M, Xc, q, log(M, q, q̂)) + vee!(LieAlgebra(M), Xc, log(M, q, q̂)) return Xc end @@ -173,11 +174,11 @@ Base.@kwdef struct PackedPose3Pose3XYYaw <: AbstractPackedFactor Z::PackedSamplableBelief end -function convert(::Type{<:Pose3Pose3XYYaw}, d::PackedPose3Pose3XYYaw) +function DFG.unpack(d::PackedPose3Pose3XYYaw) return Pose3Pose3XYYaw( convert(SamplableBelief, d.Z)) end -function convert(::Type{PackedPose3Pose3XYYaw}, d::Pose3Pose3XYYaw) +function DFG.pack(d::Pose3Pose3XYYaw) return PackedPose3Pose3XYYaw( convert(PackedSamplableBelief, d.Z)) end @@ -207,7 +208,7 @@ struct Pose3Pose3Rotation{T <: SamplableBelief} <: IIF.AbstractManifoldMinimize end Pose3Pose3Rotation(z::SamplableBelief) = Pose3Pose3Rotation(z, (4,5,6)) -getManifold(::Pose3Pose3Rotation) = SpecialOrthogonalGroup(3) +DFG.getManifold(::Type{<:Pose3Pose3Rotation}) = SpecialOrthogonalGroup(3) function (cfo::CalcFactor{<:Pose3Pose3Rotation})(Xm, wTp, wTq ) # @@ -235,11 +236,11 @@ Base.@kwdef struct PackedPose3Pose3Rotation <: AbstractPackedFactor Z::PackedSamplableBelief end -function convert(::Type{<:Pose3Pose3Rotation}, d::PackedPose3Pose3Rotation) +function DFG.unpack(d::PackedPose3Pose3Rotation) return Pose3Pose3Rotation( convert(SamplableBelief, d.Z)) end -function convert(::Type{PackedPose3Pose3Rotation}, d::Pose3Pose3Rotation) +function DFG.pack(d::Pose3Pose3Rotation) return PackedPose3Pose3Rotation( convert(PackedSamplableBelief, d.Z)) end diff --git a/src/factors/PartialPriorPose2.jl b/src/factors/PartialPriorPose2.jl index 3f610fdf..6e030418 100644 --- a/src/factors/PartialPriorPose2.jl +++ b/src/factors/PartialPriorPose2.jl @@ -10,7 +10,7 @@ Base.@kwdef struct PartialPriorYawPose2{T <: IIF.SamplableBelief} <: IIF.Abstrac end PartialPriorYawPose2(Z::SamplableBelief) = PartialPriorYawPose2(;Z) -getManifold(::PartialPriorYawPose2) = RealCircleGroup() # SpecialEuclidean(2) +DFG.getManifold(::Type{<:PartialPriorYawPose2}) = CircleGroup(ℝ) # SpecialEuclidean(2) function getSample(cf::CalcFactor{<:PartialPriorYawPose2}) diff --git a/src/factors/Polar.jl b/src/factors/Polar.jl index 3d10a100..800fd0fb 100644 --- a/src/factors/Polar.jl +++ b/src/factors/Polar.jl @@ -8,7 +8,7 @@ struct Polar <: IIF.InferenceVariable dims::Int end -getManifold(::InstanceType{Polar}) = BearingRange_Manifold +DFG.getManifold(::InstanceType{Polar}) = BearingRange_Manifold """ $TYPEDEF diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index 58c7da9a..1134636b 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -30,13 +30,13 @@ Base.@kwdef struct Pose2Pose2{T<:IIF.SamplableBelief} <: IIF.AbstractManifoldMin Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0])) end -DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +DFG.getManifold(::InstanceType{Pose2Pose2}) = SOnxRn_MetricManifold(2) Pose2Pose2(::UniformScaling) = Pose2Pose2() function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M - M = getManifold(Pose2) + M = getManifold(Pose2Pose2) X̂ = log(M, p, q) return vee(M, p, X - X̂) # TODO check sign end diff --git a/src/factors/Pose2Point2.jl b/src/factors/Pose2Point2.jl index d0d041e6..034ec122 100644 --- a/src/factors/Pose2Point2.jl +++ b/src/factors/Pose2Point2.jl @@ -17,18 +17,20 @@ end Pose2Point2(Z::SamplableBelief) = Pose2Point2(;Z) # TODO verify this is right for partial factor -getManifold(::InstanceType{Pose2Point2}) = getManifold(Point2) +DFG.getManifold(::InstanceType{Pose2Point2}) = getManifold(Point2) # define the conditional probability constraint function (cfo::CalcFactor{<:Pose2Point2})(p_Xpq, w_T_p, w_Tl_q ) # - M = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) + M = SOnxRn_MetricManifold(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) + # w_H_qhat = affine_matrix(M, _w_T_p) * affine_matrix(M, p_T_qhat) #FIXME check this + w_H_qhat = LieGroups.compose(M, _w_T_p, 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] @@ -46,11 +48,11 @@ Base.@kwdef struct PackedPose2Point2 <: AbstractPackedFactor Z::PackedSamplableBelief end -function convert(::Type{PackedPose2Point2}, obj::Pose2Point2{T}) where {T <: IIF.SamplableBelief} +function DFG.pack(obj::Pose2Point2) return PackedPose2Point2(convert(PackedSamplableBelief, obj.Z)) end # TODO -- should not be resorting to string, consider specialized code for parametric distribution types and KDEs -function convert(::Type{Pose2Point2}, packed::PackedPose2Point2) +function DFG.unpack(packed::PackedPose2Point2) Pose2Point2(convert(SamplableBelief, packed.Z)) end diff --git a/src/factors/Poses.jl b/src/factors/Poses.jl index e1d050d6..a415ff18 100644 --- a/src/factors/Poses.jl +++ b/src/factors/Poses.jl @@ -1,13 +1,21 @@ +# NOTE we follow `residual = measurement - prediction` as a convention + +# NOTE ON PRIORS ON MANIFOLDS: +# For prior factors, we compute the residual as log(M, p, m), which gives a tangent vector at the current state estimate p +# pointing toward the measurement m. This means the residual lives in the tangent space at p (TₚM). +# +# - The optimizer linearizes and updates in the tangent space at the current estimate p. + +# Note on injectivity_radius +# If the measurement is outside the injectivity radius of the manifold, the log will not be valid. +# even if we use Xq = log(M, q, q̂), we need to transport Xq to Xp, parallel_transport_to still uses the log map. function (cf::CalcFactor{<:PriorPose2})(m, p) M = getManifold(PriorPose2) - X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? Also update the rest if this is wrong. + X = log(M, p, m) # the residual is calculated at the current state estimate p, ie. X ∈ TₚM return vee(LieAlgebra(M), X) end -## NOTE likely deprecated comparitors, see DFG compareFields, compareAll instead -compare(a::PriorPose2, b::PriorPose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) - function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) # X ∈ TₚM, X̂ ∈ TₚM, p,q ∈ M M = getManifold(Pose2Pose2) @@ -15,9 +23,6 @@ function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) return vee(LieAlgebra(M), X - X̂) end -# FIXME, rather have separate compareDensity functions -compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) - function (cf::CalcFactor{<:PriorPose3})(m, p) M = getManifold(PriorPose3) return vee(LieAlgebra(M), log(M, p, m)) @@ -30,6 +35,11 @@ function (cf::CalcFactor{<:Pose3Pose3})(X, p::ArrayPartition{T}, q) where {T} return Xc end +# FIXME, rather have separate compareDensity functions +compare(a::Pose2Pose2, b::Pose2Pose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) +## NOTE likely deprecated comparitors, see DFG compareFields, compareAll instead +compare(a::PriorPose2, b::PriorPose2; tol::Float64 = 1e-10) = compareDensity(a.Z, b.Z) + ## #TODO is this manifold not SO3 DFG.@defObservationType Pose3Pose3RotOffset AbstractManifoldMinimize SOnxRn_MetricManifold(3) @@ -77,3 +87,9 @@ function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) whe Xc::SVector{6, T} = vee(LieAlgebra(M), log(M, q, q̂)) return SVector{6, T}(normalize(Xc[1:3])..., Xc[4:6]...) end + + +# FIXME needed until AMP#41 is done hopefully can be removed soon 🐛💥 +# Base.convert(::Type{<:Tuple}, ::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) +AMP._manifoldtuple(::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) +AMP._manifoldtuple(::typeof(SOnxRn_MetricManifold(3))) = (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular) diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index c44f106d..9ec349be 100644 --- a/src/factors/PriorPose2.jl +++ b/src/factors/PriorPose2.jl @@ -27,9 +27,9 @@ 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) - X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? - return vee(M, p, X) + M = getManifold(PriorPose2) + X = log(M, p, m) # Currently X ∈ TₚM, #TODO should it be TₘM? Also update the rest if this is wrong. + return vee(LieAlgebra(M), X) end #TODO Serialization of reference point p @@ -41,11 +41,11 @@ $(TYPEDEF) Base.@kwdef struct PackedPriorPose2 <: AbstractPackedFactor Z::PackedSamplableBelief end -function convert(::Type{PackedPriorPose2}, d::PriorPose2) - return PackedPriorPose2(convert(PackedSamplableBelief, d.Z)) +function DFG.pack(d::PriorPose2) + return PackedPriorPose2(packDistribution(d.Z)) end -function convert(::Type{PriorPose2}, d::PackedPriorPose2) - return PriorPose2(convert(SamplableBelief, d.Z)) +function DFG.unpack(d::PackedPriorPose2) + return PriorPose2(unpackDistribution(d.Z)) end diff --git a/src/factors/PriorVelPos3.jl b/src/factors/PriorVelPos3.jl index 7b96e7af..acaaf6ff 100644 --- a/src/factors/PriorVelPos3.jl +++ b/src/factors/PriorVelPos3.jl @@ -10,16 +10,8 @@ Example: PriorVelPos3( MvNormal(zeros(6), Matrix(Diagonal(ones(6).^2))) ) ``` """ -Base.@kwdef struct PriorVelPos3{T <: SamplableBelief} <: IIF.AbstractPrior - Z::T = MvNormal(zeros(3), diagm([1;1;0.1])) -end - -DFG.getManifold(::InstanceType{PriorVelPos3}) = getManifold(VelPos3) - +DFG.@defObservationType PriorVelPos3 PriorObservation TranslationGroup(3) × TranslationGroup(3) -# 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{<:PriorVelPos3})( m::ArrayPartition, @@ -35,16 +27,4 @@ end #TODO Serialization of reference point p ## Serialization support -""" -$(TYPEDEF) -""" -Base.@kwdef struct PackedPriorVelPos3 <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{PackedPriorVelPos3}, d::PriorVelPos3) - return PackedPriorVelPos3(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{PriorVelPos3}, d::PackedPriorVelPos3) - return PriorVelPos3(convert(SamplableBelief, d.Z)) -end diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl index ebfb526d..75971ca0 100644 --- a/src/factors/Range2D.jl +++ b/src/factors/Range2D.jl @@ -4,12 +4,7 @@ """ $(TYPEDEF) """ -mutable struct Point2Point2Range{D <: IIF.SamplableBelief} <: IncrementalInference.AbstractManifoldMinimize # AbstractRelativeMinimize - Z::D -end - -getManifold(::InstanceType{Point2Point2Range}) = TranslationGroup(1) - +DFG.@defObservationType Point2Point2Range RelativeObservation TranslationGroup(1) function (cfo::CalcFactor{<:Point2Point2Range})(rho, xi, lm) # Basically `EuclidDistance` @@ -19,19 +14,6 @@ end passTypeThrough(d::FunctionNodeData{Point2Point2Range}) = d -""" -$(TYPEDEF) -""" -Base.@kwdef mutable struct PackedPoint2Point2Range <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{PackedPoint2Point2Range}, d::Point2Point2Range) - return PackedPoint2Point2Range(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{Point2Point2Range}, d::PackedPoint2Point2Range) - return Point2Point2Range(convert(SamplableBelief, d.Z)) -end - """ @@ -45,7 +27,7 @@ Base.@kwdef struct Pose2Point2Range{T <: IIF.SamplableBelief} <: IIF.AbstractMan end Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range(;Z) -getManifold(::Pose2Point2Range) = TranslationGroup(1) +DFG.getManifold(::Type{<:Pose2Point2Range}) = TranslationGroup(1) function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi::ArrayPartition, lm) @@ -56,9 +38,9 @@ end Base.@kwdef struct PackedPose2Point2Range <: AbstractPackedFactor Z::PackedSamplableBelief end -function convert(::Type{PackedPose2Point2Range}, d::Pose2Point2Range) +function DFG.pack(d::Pose2Point2Range) return PackedPose2Point2Range(convert(PackedSamplableBelief, d.Z)) end -function convert(::Type{Pose2Point2Range}, d::PackedPose2Point2Range) +function DFG.unpack(d::PackedPose2Point2Range) return Pose2Point2Range(convert(SamplableBelief, d.Z)) end diff --git a/src/factors/SensorModels.jl b/src/factors/SensorModels.jl index 090ba5a8..e5de9250 100644 --- a/src/factors/SensorModels.jl +++ b/src/factors/SensorModels.jl @@ -28,7 +28,7 @@ mutable struct LinearRangeBearingElevation <: IIF.AbstractManifoldMinimize LinearRangeBearingElevation( r::Tuple{Float64,Float64}, b::Tuple{Float64,Float64}; elev=Uniform(-0.25133,0.25133)) = new(Normal(r...),Normal(b...),elev, reuseLBRA[reuseLBRA(0) for i in 1:Threads.nthreads()] ) end -getManifold(::LinearRangeBearingElevation) = Euclidean(3)#FIXME +DFG.getManifold(::Type{<:LinearRangeBearingElevation}) = Euclidean(3)#FIXME function (cfo::CalcFactor{<:LinearRangeBearingElevation})(meas, _pose, _landm) #FIXME update to manifolds, quick fix convert for now diff --git a/src/factors/VelAlign.jl b/src/factors/VelAlign.jl index 452d7e1a..cbedcf58 100644 --- a/src/factors/VelAlign.jl +++ b/src/factors/VelAlign.jl @@ -3,18 +3,11 @@ """ $(TYPEDEF) """ -Base.@kwdef struct VelAlign{D <: IIF.SamplableBelief} <: AbstractManifoldMinimize - """ Measurement vector of [1;0;0] here implies body velocity is assumed forward along body x-axis """ - Z::D = MvNormal([1;0;0.0],diagm([0.1;0.1;0.1])) -end - -getManifold(::InstanceType{VelAlign}) = TranslationGroup(3) - - +DFG.@defObservationType VelAlign RelativeObservation TranslationGroup(3) function IncrementalInference.preambleCache( dfg::AbstractDFG, - vars::AbstractVector{<:DFGVariable}, + vars::AbstractVector{<:VariableCompute}, ::VelAlign, ) # TODO, obsolete -- replace with NamedTuple submanifold checks @@ -40,19 +33,3 @@ function (cf::CalcFactor{<:VelAlign})( q_V = cf.cache.q_rot(w_T_q)' * cf.cache.q_vel(w_T_q) p_V - p_R_q * q_V end - - -""" -$(TYPEDEF) - -Serialization type for `VelAlign`. -""" -Base.@kwdef struct PackedVelAlign <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{VelAlign}, d::PackedVelAlign) - return VelAlign( convert(SamplableBelief, d.Z) ) -end -function convert(::Type{PackedVelAlign}, d::VelAlign) - return PackedVelAlign( convert(PackedSamplableBelief, d.Z) ) -end diff --git a/src/factors/VelPoint2D.jl b/src/factors/VelPoint2D.jl index 4b086175..227980e2 100644 --- a/src/factors/VelPoint2D.jl +++ b/src/factors/VelPoint2D.jl @@ -6,11 +6,7 @@ """ $(TYPEDEF) """ -mutable struct VelPoint2VelPoint2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize #RelativeMinimize - Z::T -end - -getManifold(::InstanceType{VelPoint2VelPoint2}) = TranslationGroup(4) +DFG.@defObservationType VelPoint2VelPoint2 RelativeObservation TranslationGroup(4) function (cfo::CalcFactor{<:VelPoint2VelPoint2})(z, xi, xj) # @@ -55,18 +51,3 @@ function (cfo::CalcFactor{<:VelPoint2VelPoint2})(z, xi, xj) # return residual end - - -""" -$(TYPEDEF) -""" -Base.@kwdef struct PackedVelPoint2VelPoint2 <: AbstractPackedFactor - Z::PackedSamplableBelief -end - -function convert(::Type{PackedVelPoint2VelPoint2}, d::VelPoint2VelPoint2) - return PackedVelPoint2VelPoint2(convert(PackedSamplableBelief, d.Z)) -end -function convert(::Type{VelPoint2VelPoint2}, d::PackedVelPoint2VelPoint2) - return VelPoint2VelPoint2(convert(SamplableBelief, d.Z)) -end diff --git a/src/factors/VelPosRotVelPos.jl b/src/factors/VelPosRotVelPos.jl index df97ed7f..a6db249c 100644 --- a/src/factors/VelPosRotVelPos.jl +++ b/src/factors/VelPosRotVelPos.jl @@ -3,16 +3,7 @@ """ $(TYPEDEF) """ -Base.@kwdef struct VelPosRotVelPos{D <: IIF.SamplableBelief} <: AbstractManifoldMinimize - Z::D = MvNormal(zeros(3),diagm([0.1;0.1;0.1])) -end - -getManifold(::InstanceType{VelPosRotVelPos}) = ProductGroup( - ProductManifold( - TranslationGroup(3), - TranslationGroup(3) - ) -) +DFG.@defObservationType VelPosRotVelPos RelativeObservation TranslationGroup(3) × TranslationGroup(3) function (cf::CalcFactor{<:VelPosRotVelPos})( X_vp, @@ -24,19 +15,3 @@ function (cf::CalcFactor{<:VelPosRotVelPos})( X_vp.x[2] .- (q.x[3] .- p.x[2]); ] end - - -""" -$(TYPEDEF) - -Serialization type for `VelPosRotVelPos`. -""" -Base.@kwdef struct PackedVelPosRotVelPos <: AbstractPackedFactor - Z::PackedSamplableBelief -end -function convert(::Type{VelPosRotVelPos}, d::PackedVelPosRotVelPos) - return VelPosRotVelPos( convert(SamplableBelief, d.Z) ) -end -function convert(::Type{PackedVelPosRotVelPos}, d::VelPosRotVelPos) - return PackedVelPosRotVelPos( convert(PackedSamplableBelief, d.Z) ) -end \ No newline at end of file diff --git a/src/factors/VelPose2D.jl b/src/factors/VelPose2D.jl index 24bc919d..32272d7b 100644 --- a/src/factors/VelPose2D.jl +++ b/src/factors/VelPose2D.jl @@ -9,21 +9,16 @@ struct VelPose2VelPose2{T1 <: IIF.SamplableBelief,T2 <: IIF.SamplableBelief} <: end VelPose2VelPose2(z1::SamplableBelief, z2::SamplableBelief) = VelPose2VelPose2(Pose2Pose2(z1), z2) -getManifold(::InstanceType{VelPose2VelPose2}) = getManifold(DynPose2) +DFG.getManifold(::InstanceType{VelPose2VelPose2}) = getManifold(DynPose2) preableCache(::AbstractDFG, ::AbstractVector{Symbol}, ::VelPose2VelPose2) = zeros(3) function getSample(cf::CalcFactor{<:VelPose2VelPose2}) #Pose2 part - Xc = rand(cf.factor.Zpose.Z) - M = getManifold(Pose2) - ϵ = getPointIdentity(Pose2) - # ϵ = Manifolds.Identity(M) - Xpose = hat(M, ϵ, Xc) - #velocity part - Xvel = rand(cf.factor.Zvel) - - return ArrayPartition(Xpose, Xvel) + Xc = [rand(cf.factor.Zpose.Z); rand(cf.factor.Zvel)] + M = getManifold(VelPose2VelPose2) + X = hat(LieAlgebra(M), Xc) + return exp(M, X) end @@ -46,19 +41,19 @@ function (cf::CalcFactor{<:VelPose2VelPose2})(X, p, q) # pose_res = Vector{Manifolds.number_eltype(X)}(undef, 3) #Pose2 part - M1 = getManifold(Pose2) - X1 = submanifold_component(X,1) - p1 = submanifold_component(p,1) - q1 = submanifold_component(q,1) - ϵ1 = identity_element(M1, p1) - q̂1 = Manifolds.compose(M1, p1, exp(M1, ϵ1, X1)) + M1 = getManifold(Pose2Pose2) + X1 = ArrayPartition(X.x[1], X.x[2])#submanifold_component(X,1) + p1 = ArrayPartition(p.x[1], p.x[2])#submanifold_component(p,1) + q1 = ArrayPartition(q.x[1], q.x[2])#submanifold_component(q,1) + ϵ1 = getPointIdentity(M1) + q̂1 = exp(M1, p1, X1) vee!(M1, pose_res, q1, log(M1, q1, q̂1)) #velocity part dt = Dates.value(cf.fullvariables[2].nstime - cf.fullvariables[1].nstime)*1e-9 - X2 = submanifold_component(X,2) - p2 = submanifold_component(p,2) - q2 = submanifold_component(q,2) + X2 = submanifold_component(X,3) + p2 = submanifold_component(p,3) + q2 = submanifold_component(q,3) # bDXij = TransformUtils.R(-wxi[3])*wDXij bDXij = transpose(submanifold_component(p1,2))*(q2 .- p2) diff --git a/src/manifolds/SOnxRn_MetricManifold.jl b/src/manifolds/SOnxRn_MetricManifold.jl deleted file mode 100644 index e71255ab..00000000 --- a/src/manifolds/SOnxRn_MetricManifold.jl +++ /dev/null @@ -1,100 +0,0 @@ - -using Manifolds: RiemannianMetric, MetricManifold, AbstractBasis -using ManifoldsBase: submanifold_components - -# Left Invariant Rigid Body Kinematics Metric CrokeKumar eq 61. -# A family of left invariant metrics: -# G = [αI 0; 0 βI] -# where α and β are arbitrary constants, satisfies all the equations (80). -# This are the only left-invariant metrics which are compatible with the acceleration connection. -# Can we add α and β as parameters to the metric? -struct LeftInvariantKinematicMetric <: RiemannianMetric end - -SOnxRn_MetricManifold(n) = MetricManifold(SpecialEuclideanGroup(n; variant=:right), LeftInvariantKinematicMetric()) - -SOnxRn_MetricManifoldType = Union{typeof(SOnxRn_MetricManifold(2)), typeof(SOnxRn_MetricManifold(3))} - -# geodesics for metric (61) are the same as geodesics on the product manifold SO(3)×IR3 -function Manifolds.exp(M::SOnxRn_MetricManifoldType, p, X) - G = base_manifold(M) - ε = identity_element(M) #, typeof(p)) - return LieGroups.compose(G, p, exp(base_manifold(G), ε, X)) -end - -function Manifolds.exp!(M::SOnxRn_MetricManifoldType, q, p, X) - G = base_manifold(M) - ε = identity_element(M) #, typeof(p)) - return LieGroups.compose!(G, q, p, exp(base_manifold(G), ε, X)) -end - -function Manifolds.log(M::SOnxRn_MetricManifoldType, p, q) - G = base_manifold(M) - ε = identity_element(M) #, typeof(p)) - X = log(base_manifold(G), ε, LieGroups.compose(G, inv(G, p), q)) - return X -end - -function Manifolds.log!(M::SOnxRn_MetricManifoldType, X, p, q) - G = base_manifold(M) - ε = identity_element(M) #, typeof(p)) - log!(base_manifold(G), X, ε, LieGroups.compose(G, inv(G, p), q)) - return X -end - -function Manifolds.inner(M::SOnxRn_MetricManifoldType, p, X, Y) - Xtr = submanifold_components(M, X)[1] - XRo = submanifold_components(M, X)[2] - Ytr = submanifold_components(M, Y)[1] - YRo = submanifold_components(M, Y)[2] - # Metric on Chirikjian, p35 W = diagm([1,1,2]) - return dot(Xtr,Ytr) + dot(XRo, YRo)/2 -end - -function Manifolds.hat(M::SOnxRn_MetricManifoldType, p, X::AbstractVector, T=typeof(p)) - return hat(LieAlgebra(base_manifold(M)), X, T) -end - -function ManifoldsBase.get_vector( - M::SOnxRn_MetricManifoldType, g, c, B::AbstractBasis{<:Any,TangentSpaceType} -) - G = base_manifold(M) - return get_vector( - LieAlgebra(G), - c, - B; - tangent_vector_type=ManifoldsBase.tangent_vector_type(G, typeof(g)), - ) -end - -function ManifoldsBase.get_vector!( - M::SOnxRn_MetricManifoldType, X, g, c, B::AbstractBasis{<:Any,TangentSpaceType} -) - G = base_manifold(M) - return get_vector!(LieAlgebra(G), X, c, B) -end - -function ManifoldsBase.get_coordinates( - M::SOnxRn_MetricManifoldType, g, X, B::AbstractBasis{<:Any,TangentSpaceType} -) - G = base_manifold(M) - return get_coordinates(LieAlgebra(G), X, B) -end - -#FIXME: maybe replace this with identity_element -# call into base_manifold's identity_element, but got ambiguities. -Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) -Manifolds.identity_element(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) - -#FIXME remove, only temporary workaround as first signiture is used in many places -# @deprecate LieGroups.identity_element(G::AbstractLieGroup, p) identity_element(G, typeof(p)) false - -# FIXME why is this still needed, hopefully can be removed soon 🐛💥 -# Base.convert(::Type{<:Tuple}, ::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) -AMP._manifoldtuple(::typeof(SOnxRn_MetricManifold(2))) = (:Euclid,:Euclid,:Circular) -AMP._manifoldtuple(::typeof(SOnxRn_MetricManifold(3))) = (:Euclid,:Euclid,:Euclid,:Circular,:Circular,:Circular) - -#FIXME -DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(2))) = ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) -DFG.getPointIdentity(::typeof(SOnxRn_MetricManifold(3))) = ArrayPartition(SA[0,0,0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) - -LieGroups.LieAlgebra(G::SOnxRn_MetricManifoldType) = LieAlgebra(base_manifold(G)) \ No newline at end of file diff --git a/src/services/AdditionalUtils.jl b/src/services/AdditionalUtils.jl index 024ef0be..1a506d7f 100644 --- a/src/services/AdditionalUtils.jl +++ b/src/services/AdditionalUtils.jl @@ -51,7 +51,7 @@ function makePosePoseFromHomography( covar=diagm([1,1,1,0.1,0.1,0.1].^2) ) len = size(pHq,1)-1 - M = SpecialEuclidean(len; vectors=HybridTangentRepresentation()) + M = SOnxRn_MetricManifold(len) e0 = ArrayPartition(SVector(0,0,0.), SMatrix{len,len}(1,0,0,0,1,0,0,0,1.)) pTq = ArrayPartition(SVector(pHq[1:len,end]...), SMatrix{len,len}(pHq[1:len,1:len])) e0_Cpq = vee(M, e0, log(M, e0, pTq)) diff --git a/src/services/FixmeManifolds.jl b/src/services/FixmeManifolds.jl index dda2dd49..76ee37fe 100644 --- a/src/services/FixmeManifolds.jl +++ b/src/services/FixmeManifolds.jl @@ -31,7 +31,7 @@ end function Statistics.mean(::typeof(SE2E2_Manifold), pts::AbstractVector) se2_ = (d->ArrayPartition(submanifold_component(d, 1), submanifold_component(d, 2))).(pts) - mse2 = mean(Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()), se2_) + mse2 = mean(SOnxRn_MetricManifold(2), se2_) e2_ = (d->ArrayPartition(submanifold_component(d, 3))).(pts) me2 = mean(Euclidean(2), e2_) ArrayPartition(submanifold_component(mse2, 1), submanifold_component(mse2, 2), submanifold_component(me2, 1)) diff --git a/src/services/ManifoldUtils.jl b/src/services/ManifoldUtils.jl index 05ed3bc2..83a0cea2 100644 --- a/src/services/ManifoldUtils.jl +++ b/src/services/ManifoldUtils.jl @@ -6,20 +6,25 @@ function homography_to_coordinates( - M::typeof(SpecialEuclidean(3; vectors=HybridTangentRepresentation())), - pHq::AbstractMatrix{<:Real} + ::typeof(SpecialEuclideanGroup(3; variant=:right)), + H::AbstractMatrix{<:Real} ) - Mr = M.manifold[2] - e0 = Identity(Mr) - [pHq[1:3,4]; vee(Mr, e0, log(Mr, e0, pHq[1:3,1:3]))] + @warn "TODO: maybe deprecate homography_to_coordinates" + G = SpecialOrthogonalGroup(3) + [H[1:3,4]; vee(LieAlgebra(G), log(G, H[1:3,1:3]))] end function coordinates_to_homography( - M::typeof(SpecialEuclidean(3; vectors=HybridTangentRepresentation())), - pCq::AbstractVector + ::typeof(SpecialEuclideanGroup(3; variant=:right)), + c::AbstractVector ) - e0 = Identity(M) - affine_matrix(M, exp(M,e0,hat(M,e0,pCq))) + @warn "TODO: maybe deprecate coordinates_to_homography" + G = SpecialOrthogonalGroup(3) + H = zeros(4, 4) + H[1:3, 1:3] .= exp(G, hat(LieAlgebra(G), c[4:6])) + H[1:3, 4] = c[1:3] + H[4, 4] = 1.0 + return H end diff --git a/test/inertial/testIMUDeltaFactor.jl b/test/inertial/testIMUDeltaFactor.jl index 4c36d617..01e98573 100644 --- a/test/inertial/testIMUDeltaFactor.jl +++ b/test/inertial/testIMUDeltaFactor.jl @@ -1,7 +1,7 @@ using Test -using Manifolds using StaticArrays -using Rotations +import Rotations as ROT +using Rotations: RotationVec, AngleAxis, RotZ using LinearAlgebra using DistributedFactorGraphs using RoME @@ -11,19 +11,19 @@ using StaticArrays # using ManifoldDiff ## -M = SpecialOrthogonal(3) -ΔR = exp(M, Identity(M), hat(M, Identity(M), [0.1, 0.2, 0.3])) +M = SpecialOrthogonalGroup(3) +ΔR = exp(M, hat(LieAlgebra(M), [0.1, 0.2, 0.3])) a = RotationVec(ΔR) -b = Rotations.AngleAxis(ΔR) +b = AngleAxis(ΔR) function uniform_integrate_check(gyros, accels, dt) - M_SO3 = SpecialOrthogonal(3) + M_SO3 = SpecialOrthogonalGroup(3) w_R_b = identity_element(M_SO3) w_v = zeros(3) w_r = zeros(3) for (b_g, b_a) in zip(gyros, accels) - exp!(M_SO3, w_R_b, w_R_b, hat(M_SO3, Identity(M_SO3), b_g*dt)) + exp!(M_SO3, w_R_b, w_R_b, hat(LieAlgebra(M_SO3), b_g*dt)) δw_v = w_R_b * b_a*dt δw_r = w_v*dt + 0.5*δw_v*dt @@ -48,12 +48,12 @@ M = SpecialGalileanGroup() # vΔt, aΔt, ωΔt, Δt X = hat(M, SA[0.0,0,0, 0,0,0, 0,0,1, 1] * 0.001) p = exp(M, ϵ, X) -@test log_lie(M, p) ≈ X +@test log(M, p) ≈ X Xc = SA[0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1] * 0.1 X = hat(M, Xc) -p = affine_matrix(M, exp_lie(M, X)) -affine_matrix(M, log_lie(M, exp_lie(M, X))) +p = RoME.affine_matrix(M, exp(M, X)) +RoME.affine_matrix(M, log(M, exp(M, X))) X_af = RoME.vector_affine_matrix(M, X) p_af = exp(X_af) @@ -66,14 +66,14 @@ log(p_af) Xc = SA[0.01, 0.02, 0.03, 0, 0, 0, 0.1, 0.2, 0.3, 1] * 0.001 X = hat(M, Xc) p = exp(M, ϵ, X) -@test log_lie(M, p) ≈ X -@test vee(M, log_lie(M, p)) ≈ Xc +@test log(M, p) ≈ X +@test vee(M, log(M, p)) ≈ Xc Xc = SA[0, 0, 0, 0.01, 0.02, 0.03, 0.1, 0.2, 0.3, 1] * 0.001 X = hat(M, Xc) p = exp(M, ϵ, X) -@test log_lie(M, p) ≈ X -@test vee(M, log_lie(M, p)) ≈ Xc +@test log(M, p) ≈ X +@test vee(M, log(M, p)) ≈ Xc p = ArrayPartition(SMatrix{3,3}(1.0I), SA[1.,0,0], SA[0.,0,0], 0.0) q = ArrayPartition(SMatrix{3,3}(1.0I), SA[1.,0,0], SA[0.1,0,0], 0.1) @@ -98,7 +98,7 @@ p = exp(M, ϵ, X) @test isapprox(p, ArrayPartition([1 -0.005 0.0; 0.005 1 0.0; 0 0 1], [0, 0, 0.01], [0, 0, 5.0e-5], 0.01), atol=1e-4) X_af = RoME.vector_affine_matrix(M, X) p_af = exp(X_af) -@test isapprox(affine_matrix(M, p), p_af, atol=1e-4) +@test isapprox(RoME.affine_matrix(M, p), p_af, atol=1e-4) # vΔt, aΔt, ωΔt, Δt @@ -107,7 +107,7 @@ p = exp(M, ϵ, X) @test isapprox(p, ArrayPartition([1.0 0 0; 0 1 0; 0 0 1], [0.01, 0, 0], [5e-5, 0, 0], 0.01), atol=1e-4) X_af = RoME.vector_affine_matrix(M, X) p_af = exp(X_af) -@test isapprox(affine_matrix(M, p), p_af, atol=1e-4) +@test isapprox(RoME.affine_matrix(M, p), p_af, atol=1e-4) X = hat(M, SA[1,0,0, 1,0,0.0, 0,0,0.01, 1]) @@ -118,7 +118,7 @@ isapprox(compose(M, p, exp(M, ϵ, X)), exp(M, p, X)) RoME.adjointMatrix(M, X) * vee(M,X) X_af = RoME.vector_affine_matrix(M, X) -p_af = affine_matrix(M, p) +p_af = RoME.affine_matrix(M, p) Y = p_af*X_af*inv(p_af) vee(M, ArrayPartition(Y[1:3,1:3], Y[1:3,4], Y[1:3,5], Y[4,5])) @@ -126,15 +126,15 @@ vee(M, ArrayPartition(Y[1:3,1:3], Y[1:3,4], Y[1:3,5], Y[4,5])) #testing adjoint matrix with properties Adₚ = RoME.AdjointMatrix(M, p) -q1 = compose(M, p, exp_lie(M, X)) -q2 = compose(M, exp_lie(M, hat(M, Adₚ*vee(M, X))), p) +q1 = compose(M, p, exp(M, X)) +q2 = compose(M, exp(M, hat(M, Adₚ*vee(M, X))), p) @test isapprox(q1, q2) @test isapprox(RoME.AdjointMatrix(M, inv(M, p)), inv(Adₚ)) @test isapprox( RoME.vector_affine_matrix(M, hat(M, Adₚ*vee(M, X))), - affine_matrix(M, p) * RoME.vector_affine_matrix(M, X) * affine_matrix(M, inv(M, p)) + RoME.affine_matrix(M, p) * RoME.vector_affine_matrix(M, X) * RoME.affine_matrix(M, inv(M, p)) ) ad = RoME.adjointMatrix(M, X) @@ -143,7 +143,7 @@ ad = RoME.adjointMatrix(M, X) X = hat(M, SA[0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1] * 0.1) ad = RoME.adjointMatrix(M, X) -Adₚ = RoME.AdjointMatrix(M, exp_lie(M, X)) +Adₚ = RoME.AdjointMatrix(M, exp(M, X)) @test isapprox(exp(ad), Adₚ) Y = hat(M, SA[0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 1] * 0.1) @@ -160,9 +160,9 @@ jl = RoME.Jr(M, -X) X = hat(M, SA[1,0,0, 0,0,0, 0,0,θ, 1] * 0.1) p = exp(M, ϵ, X) -M_SE3 = SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +M_SE3 = SpecialEuclideanGroup(3; variant=:right) X_SE3 = hat(M_SE3, getPointIdentity(M_SE3), SA[1,0,0, 0,0,θ] * 0.1) -p_SE3 = exp_lie(M_SE3, X_SE3) +p_SE3 = exp(M_SE3, X_SE3) @test isapprox(p.x[3], p_SE3.x[1]) ## test factor with rotation around z axis and initial velocity up @@ -189,14 +189,14 @@ fac = RoME.IMUDeltaFactor( ) # Rotation part -M_SO3 = SpecialOrthogonal(3) +M_SO3 = SpecialOrthogonalGroup(3) ΔR = identity_element(M_SO3) #NOTE internally integration is done over all measurements # at t₀, we start at identity # - the measurement at t₀ is from t₋₁ to t₀ and therefore part of the previous factor # - the measurement at t₁ is from t₀ to t₁ with the time dt of t₁ - t₀ for g in imu.gyros - exp!(M_SO3, ΔR, ΔR, hat(M_SO3, Identity(M_SO3), g*dt)) + exp!(M_SO3, ΔR, ΔR, hat(LieAlgebra(M_SO3), g*dt)) end @test isapprox(M_SO3, fac.Δ.x[1], ΔR) # Velocity part @@ -257,7 +257,7 @@ addFactor!(fg, [:x0, :x1], fac) getVariableSolverData(fg, :x0, :parametric).val[1] ≈ ArrayPartition(SA[1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0], SA[10.0, 0.0, 0.0], SA[0.0, 0.0, 0.0]) x1 = getVariableSolverData(fg, :x1, :parametric).val[1] -@test isapprox(SpecialOrthogonal(3), x1.x[1], ΔR, atol=1e-5) +@test isapprox(SpecialOrthogonalGroup(3), x1.x[1], ΔR, atol=1e-5) @test isapprox(x1.x[2], [10, 0, -1], atol=1e-3) @test isapprox(x1.x[3], [10, 0, -0.5], atol=1e-3) diff --git a/test/inertial/testODE_INS.jl b/test/inertial/testODE_INS.jl index 5011fbd2..8e3d8204 100644 --- a/test/inertial/testODE_INS.jl +++ b/test/inertial/testODE_INS.jl @@ -7,7 +7,7 @@ using Dates using Statistics using TensorCast using StaticArrays -using Manifolds +# using Manifolds using IncrementalInference using RoME @@ -24,7 +24,7 @@ using Test dt = 0.01 N = 101 w_R_b = [1. 0 0; 0 1 0; 0 0 1] -imu = RoME.generateField_InertialMeasurement_RateZ(; +imu = RoME.generateField_InertialMeasurement(; dt, N, rate = [0.01, 0, 0], @@ -39,7 +39,7 @@ p = (gyro=gyros_t, accel=accels_t) p.accel(0.9) -u0 = ArrayPartition([0.0,0,0], Matrix(getPointIdentity(SpecialOrthogonal(3))), [0.,0,0]) +u0 = ArrayPartition([0.0,0,0], Matrix(getPointIdentity(SpecialOrthogonalGroup(3))), [0.,0,0]) tspan = (0.0, 1.0) prob = ODEProblem(RoME.insKinematic!, u0, tspan, Ref(p)) @@ -47,7 +47,7 @@ prob = ODEProblem(RoME.insKinematic!, u0, tspan, Ref(p)) sol = solve(prob) last(sol) -M = SpecialOrthogonal(3) +M = SpecialOrthogonalGroup(3) @test isapprox(last(sol).x[1], [0,0,0]; atol=0.001) @test isapprox(M, last(sol).x[2], w_R_b; atol=0.001) @test isapprox(last(sol).x[3], [0,0,0]; atol=0.001) diff --git a/test/runtests.jl b/test/runtests.jl index 50e8a209..a5eef13a 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -5,11 +5,16 @@ using RoME using Test using TensorCast import Manifolds -using Manifolds: ProductManifold, SpecialEuclidean, SpecialOrthogonal, TranslationGroup, identity_element +using LieGroups +# using Manifolds: ProductManifold, SpecialEuclidean, SpecialOrthogonal, TranslationGroup, identity_element using DistributedFactorGraphs using Statistics +using LinearAlgebra +using Random using StaticArrays +using RoME: SOnxRn_MetricManifold +using ManifoldsBase: submanifold_component @error("add test for generateGraph_Beehive!, norm( simulated - default ) < tol") @@ -21,7 +26,7 @@ testfiles = [ # Inertial "inertial/testInertialDynamic.jl"; "inertial/testODE_INS.jl"; - "inertial/testIMUDeltaFactor.jl"; + "inertial/testIMUDeltaFactor.jl"; #FIXME slow # ... # "testFluxModelsPose2.jl"; @@ -34,7 +39,7 @@ testfiles = [ "testVelPos3.jl"; # tests most likely to fail on numerics - "testScalarFields.jl"; + "testScalarFields.jl"; #FIXME slow "testPoint2Point2Init.jl"; "threeDimLinearProductTest.jl"; "testPose3Pose3NH.jl"; @@ -47,7 +52,7 @@ testfiles = [ "testMultimodalRangeBearing.jl"; # restore after Bearing factors are fixed # regular tests expected to pass - "testpackingconverters.jl"; + "testpackingconverters.jl"; #FIXME for new DFG deprecations "testInflation380.jl"; "testPoint2Point2.jl"; "testParametricCovariances.jl"; @@ -62,9 +67,9 @@ testfiles = [ "testBasicPose2Stationary.jl"; "TestPoseAndPoint2Constraints.jl"; "testDynPoint2D.jl"; - "testDeltaOdo.jl"; + "testDeltaOdo.jl"; #FIXME slow "testFixedLagFG.jl"; - "testDynPose2D.jl"; + "testDynPose2D.jl"; #FIXME slow "testPartialPriorYawPose2.jl"; "TestDefaultFGInitialization.jl"; "testAccumulateFactors.jl"; @@ -82,15 +87,14 @@ testfiles = [ # "HexagonalLightGraphs.jl" # "testCameraFunctions.jl" # "testmultiplefeatures.jl" - +@testset "RoME tests" begin for (i,testf) in enumerate(testfiles) - println("[TEST $i] $testf =============================================================") - include(testf) - println("[SUCCESS] $testf") + @testset "[TEST $i] $testf" begin + println("[TEST $i] $testf =============================================================") + include(testf) + end println() println() println() end - - -# \ No newline at end of file +end diff --git a/test/testBearing2D.jl b/test/testBearing2D.jl index 694ee7ff..86d3554f 100644 --- a/test/testBearing2D.jl +++ b/test/testBearing2D.jl @@ -10,7 +10,7 @@ using Manifolds: hat @testset "Testing Bearing2D factor" begin ## -M = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) +M = RoME.SOnxRn_MetricManifold(2) ϵ = getPointIdentity(M) ps = [exp(M, ϵ, hat(M, ϵ, [0.,0,0]))] push!(ps, exp(M, ϵ, hat(M, ϵ, [5.,0,0]))) @@ -34,7 +34,7 @@ rs = [0, -pi/4, -pi/2, -3pi/4, -pi, 3pi/4, pi/2, pi/4, pi/4, -pi/4] push!(rs, pi/4 - atan(3,4)) q = [5., 5] -SO2 = SpecialOrthogonal(2) +SO2 = SpecialOrthogonalGroup(2) m = hat(SO2, getPointIdentity(SO2), [pi/4]) f = Pose2Point2Bearing(Normal(pi/4,0.05)) diff --git a/test/testBearingRange2D.jl b/test/testBearingRange2D.jl index 086bcc3e..9883c535 100644 --- a/test/testBearingRange2D.jl +++ b/test/testBearingRange2D.jl @@ -1,11 +1,12 @@ using RoME using Statistics -using Manifolds +# using Manifolds # , Distributions using Test using DistributedFactorGraphs using TensorCast import Base: convert +using LinearAlgebra ## @@ -340,7 +341,7 @@ addFactor!(fg, [:x0; :l1], p2br, graphinit=false) # check the forward convolution is working properly _pts = getPoints(propagateBelief(fg, :x0, ls(fg, :x0); N)[1]) -p_μ = mean(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), _pts) +p_μ = mean(SOnxRn_MetricManifold(2), _pts) _pts = IIF.getCoordinates.(Pose2, _pts) @cast pts[j,i] := _pts[i][j] @@ -367,8 +368,8 @@ end ## p2p2b = Pose2Point2Bearing( MvNormal([0.2,0.2,0.2], [1.0 0 0;0 1 0;0 0 1]) ) -packed = convert(PackedPose2Point2Bearing, p2p2b) -p2p2bTest = convert(Pose2Point2Bearing, packed) +packed = pack(p2p2b) +p2p2bTest = unpack(packed) @test p2p2b.Z.μ == p2p2bTest.Z.μ @test p2p2b.Z.Σ.mat == p2p2bTest.Z.Σ.mat diff --git a/test/testDidsonFunctions.jl b/test/testDidsonFunctions.jl index a19df525..a84f0531 100644 --- a/test/testDidsonFunctions.jl +++ b/test/testDidsonFunctions.jl @@ -3,7 +3,7 @@ using RoME using DistributedFactorGraphs using Distributions -using Manifolds: TranslationGroup +using LieGroups: TranslationGroup using Test ## @@ -51,7 +51,7 @@ addFactor!(fg, [:x0;:x1], meas, graphinit=false) pts = approxConv(fg, :x0x1f1, :x0) -p2 = manikde!(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), pts); +p2 = manikde!(SOnxRn_MetricManifold(3), pts); end diff --git a/test/testG2oExportSE3.jl b/test/testG2oExportSE3.jl index 82ac69ca..5f80bbcb 100644 --- a/test/testG2oExportSE3.jl +++ b/test/testG2oExportSE3.jl @@ -18,7 +18,8 @@ using Test # addFactor!(fg, [:x2;:x3], Pose3Pose3(MvNormal([1;0;0;0;0;0.],diagm(0.1*ones(6)))); graphinit=false) # saveDFG(joinpath(@__DIR__,"testdata","g2otest.tar.gz"), fg) -fg = loadDFG!(initfg(), joinpath(@__DIR__,"testdata","g2otest.tar.gz")) +# fg = loadDFG!(initfg(), joinpath(@__DIR__,"testdata","g2otest.tar.gz")) +fg = loadDFG(joinpath(@__DIR__,"testdata","g2otest.tar.gz")) setPPE!.(fg, ls(fg), :parametric) g2ofile = joinpath("/tmp", "caesar", "export.g2o") diff --git a/test/testGenericProjection.jl b/test/testGenericProjection.jl index 6ed34a3e..e33c7f83 100644 --- a/test/testGenericProjection.jl +++ b/test/testGenericProjection.jl @@ -4,7 +4,7 @@ using Test using CameraModels using RoME -using Manifolds +# using Manifolds # using ManifoldDiff # import FiniteDifferences as FD @@ -110,6 +110,7 @@ w_P3 = solveMultiviewLandmark!(fg, :w_Ph) filepath = joinpath(tempdir(), "testgeneric.tar.gz") saveDFG(filepath, fg) +@test_broken begin fg_ = loadDFG!(initfg(), filepath) Base.rm(filepath) @@ -118,7 +119,7 @@ Base.rm(filepath) w_P3 = solveMultiviewLandmark!(fg_, :w_Ph) @test isapprox([10.56;0;0], w_P3; atol=1e-3) - +end ## end diff --git a/test/testHexagonal2D_CliqByCliq.jl b/test/testHexagonal2D_CliqByCliq.jl index b38e34af..5845f267 100644 --- a/test/testHexagonal2D_CliqByCliq.jl +++ b/test/testHexagonal2D_CliqByCliq.jl @@ -55,7 +55,7 @@ M = getManifold(Pose2()) # @test 35 < sum(-0.3 .< cs[3,:] .< 0.3) μ = mean(M, getPoints(getBelief(fg, :x3))) @test isapprox(submanifold_component(μ,1), [11; 17.5], atol=3.0) -@test isapprox(SpecialOrthogonal(2), submanifold_component(μ,2), [-1 0; 0 -1], atol=0.5) +@test isapprox(SpecialOrthogonalGroup(2), submanifold_component(μ,2), [-1 0; 0 -1], atol=0.5) Σ = cov(getManifold(Pose2()), getPoints(getBelief(fg, :x3))) @test all(diag(Σ) .< [5,5,1].^2) #TODO smaller diff --git a/test/testInflation380.jl b/test/testInflation380.jl index 77f2ad95..b5a3b22c 100644 --- a/test/testInflation380.jl +++ b/test/testInflation380.jl @@ -162,8 +162,8 @@ addFactor!(fg, [:x0], PriorPose2(MvNormal(rand(prpo), pr_noise))) addVariable!(fg, :l1, Point2, tags=[:LANDMARK]) addVariable!(fg, :l2, Point2, tags=[:LANDMARK]) -p2br = Pose2Point2BearingRange(Normal(pi/4 + rand(Normal(0,σ_bearing)), σ_bearing), - Normal(sqrt(2) + rand(Normal(0,σ_range)), σ_range)) +p2br = Pose2Point2BearingRange(Normal(rand(Normal(pi/4,σ_bearing)), σ_bearing), + Normal(rand(Normal(sqrt(2),σ_range)), σ_range)) addFactor!(fg, [:x0; :l1], p2br) addVariable!(fg, :x1, Pose2) @@ -198,6 +198,7 @@ addFactor!(fg, [:x2; :l2], p2br) solveGraph!(fg); # parametric solution +IIF.autoinitParametric!(fg) IIF.solveGraphParametric!(fg) diff --git a/test/testMultimodalRangeBearing.jl b/test/testMultimodalRangeBearing.jl index d0dffd6d..d285da1c 100644 --- a/test/testMultimodalRangeBearing.jl +++ b/test/testMultimodalRangeBearing.jl @@ -1,8 +1,9 @@ using RoME using Test using DistributedFactorGraphs -using Manifolds: TranslationGroup +using LieGroups: TranslationGroup using TensorCast +using Random # using RoMEPlotting, Distributions import IncrementalInference: getSample @@ -86,7 +87,7 @@ end @testset "test multimodal landmark locations are computed correclty..." begin - +Random.seed!(123) # Set the seed for reproducibility ## # Start with an empty graph @@ -101,10 +102,10 @@ addVariable!(fg, :l1, Point2, tags=[:LANDMARK;]) addFactor!(fg, [:l1], PriorPoint2(MvNormal([40.0;0.0], diagm([1.0;1.0].^2))) ) addVariable!(fg, :l2, Point2, tags=[:LANDMARK;]) -addFactor!(fg, [:l2;], NorthSouthPartial(Normal(0,1.0))) +addFactor!(fg, [:l2;], NorthSouthPartial(Normal(0,0.1))) # addFactor!(fg, [:l2], PriorPose2(MvNormal([30.0;0.0], diagm([1.0;1.0].^2))) ) -p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,1.0)) +p2br = Pose2Point2BearingRange(Normal(0,0.1),Normal(20.0,0.1)) addFactor!(fg, [:x0; :l1; :l2], p2br, multihypo=[1.0; 0.5; 0.5]) # solve the graph diff --git a/test/testParametric.jl b/test/testParametric.jl index 182bfd22..51ae9dc3 100644 --- a/test/testParametric.jl +++ b/test/testParametric.jl @@ -1,6 +1,6 @@ using RoME using Test -using Manifolds +using LieGroups # using RoMEPlotting @@ -46,11 +46,11 @@ M = getManifold(Pose2) # @test isapprox(vardict[:x0].val, [10, 10, -pi], atol = 1e-3) # @test isapprox(vardict[:x4].val, [10, 10, -pi], atol = 1e-3) ϵ = getPointIdentity(M) -@test isapprox(M, r[1], exp(M, ϵ, hat(M,ϵ,[10, 10, -pi])), atol = 1e-3) -@test isapprox(M, r[2], exp(M, ϵ, hat(M,ϵ,[0, 10, -pi/2])), atol = 1e-3) -@test isapprox(M, r[3], exp(M, ϵ, hat(M,ϵ,[0, 0, 0])), atol = 1e-3) -@test isapprox(M, r[4], exp(M, ϵ, hat(M,ϵ,[10, 0, pi/2])), atol = 1e-3) -@test isapprox(M, r[5], exp(M, ϵ, hat(M,ϵ,[10, 10, -pi])), atol = 1e-3) +@test isapprox(M, r[1], exp(M, ϵ, hat(LieAlgebra(M), [10, 10, -pi])), atol = 1e-3) +@test isapprox(M, r[2], exp(M, ϵ, hat(LieAlgebra(M), [0, 10, -pi/2])), atol = 1e-3) +@test isapprox(M, r[3], exp(M, ϵ, hat(LieAlgebra(M), [0, 0, 0])), atol = 1e-3) +@test isapprox(M, r[4], exp(M, ϵ, hat(LieAlgebra(M), [10, 0, pi/2])), atol = 1e-3) +@test isapprox(M, r[5], exp(M, ϵ, hat(LieAlgebra(M), [10, 10, -pi])), atol = 1e-3) # IIF.updateParametricSolution(fg, vardict) # pl = plotSLAM2D(fg; lbls=true, solveKey=:parametric, point_size=4pt, drawPoints=false, drawContour=false) @@ -171,7 +171,7 @@ IIF.initParametricFrom!(fg) PM, varLabels, r, Σ = IIF.solveGraphParametric(fg) #autodiff=:finite) -@test isapprox(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), r[1], ArrayPartition([2, 0.], [0 -1; 1 0.]), atol = 1e-3) +@test isapprox(RoME.SOnxRn_MetricManifold(2), r[1], ArrayPartition([2, 0.], [0 -1; 1 0.]), atol = 1e-3) @test isapprox(r[2], [1, 1], atol = 1e-3) @test isapprox(r[3], [1, -1], atol = 1e-3) diff --git a/test/testParametricSimulated.jl b/test/testParametricSimulated.jl index 29179865..63694afa 100644 --- a/test/testParametricSimulated.jl +++ b/test/testParametricSimulated.jl @@ -6,7 +6,6 @@ using Test using RoME using DistributedFactorGraphs -using Manifolds: hat, exp ## @testset "ensure solveParametricBinary is working" begin @@ -31,10 +30,10 @@ pp2 = Pose2Pose2( MvNormal([0;0;-pi+0.01], diagm(0.03*ones(3)) )) ## -M = getManifold(Pose2) +M = getManifold(Pose2Pose2) ϵ = getPointIdentity(Pose2) -X = hat(M, ϵ, [0;0;-pi]) #measurement +X = hat(LieAlgebra(M), [0;0;-pi], typeof(ϵ)) #measurement p = ϵ # variable from q = ϵ # variable to @test isapprox( abs.(calcFactorResidualTemporary(pp2, (Pose2, Pose2), X, (p, q))), [0;0;pi] ) @@ -43,7 +42,8 @@ q = exp(M, ϵ, hat(M, ϵ, [0;0;-pi])) # variable to @test isapprox( calcFactorResidualTemporary(pp2, (Pose2, Pose2), X, (p, q)), [0;0;0], atol=1e-14 ) q = exp(M, ϵ, hat(M, ϵ, [0;0;pi])) # variable to -@test isapprox( calcFactorResidualTemporary(pp2, (Pose2, Pose2), X, (p, q)), [0;0;0], atol=1e-14 ) +#FIXME this test is probably not valid as the angle is outside the injectivity radius +@test_broken isapprox( calcFactorResidualTemporary(pp2, (Pose2, Pose2), X, (p, q)), [0;0;0], atol=1e-14 ) # wXjhat = SE2(zeros(3))*SE2([0;0;-pi]) diff --git a/test/testPartialPose3.jl b/test/testPartialPose3.jl index ff7c7ea4..e34ed0fc 100644 --- a/test/testPartialPose3.jl +++ b/test/testPartialPose3.jl @@ -4,11 +4,11 @@ using Statistics using RoME using Test -using Manifolds: hat, vee, identity_element, SpecialOrthogonal, SpecialEuclidean +# using Manifolds: hat, vee, identity_element, SpecialOrthogonal, SpecialEuclidean import Manifolds using TensorCast using DistributedFactorGraphs -using Rotations +using Rotations: RotX, RotY, RotZ, RotZYX, RotXYZ ## @@ -35,7 +35,7 @@ end fg = initfg() -M=SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +M=SOnxRn_MetricManifold(3) N = 100 fg.solverParams.N = N fg.solverParams.graphinit = false @@ -360,7 +360,7 @@ end @test size(val, 1) == 6 @test size(val, 2) == N -estmu1mean = Statistics.mean(val[collect(DFG.getSolverData(f1).fnc.usrfnc!.partial),:],dims=2) +estmu1mean = Statistics.mean(val[collect(DFG.getObservation(f1).partial),:],dims=2) # estmu2mean = Statistics.mean(val[collect(DFG.getSolverData(f2).fnc.usrfnc!.partial),:],dims=2) estmu2mean = Statistics.mean(val[[1,2,6],:],dims=2) @@ -486,7 +486,7 @@ end ## -M = SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +M = SOnxRn_MetricManifold(3) mpts = getPoints(fg[1], :x4) mu_fg1 = mean(M, mpts) @@ -508,7 +508,7 @@ end M3 = getManifold(Pose3) ϵ3 = getPointIdentity(Pose3) -M2 = SpecialOrthogonal(3) +M2 = SpecialOrthogonalGroup(3) ϵ2 = getPointIdentity(M2) ϕ = 0.1 diff --git a/test/testPose3.jl b/test/testPose3.jl index bf8f74ec..e5439a1e 100644 --- a/test/testPose3.jl +++ b/test/testPose3.jl @@ -1,6 +1,6 @@ # using Revise using RoME -using Manifolds +using LieGroups using StaticArrays using Test using StaticArrays @@ -9,9 +9,7 @@ using StaticArrays @testset "test SE(3) coordinates to homography and back" begin ## -M = getManifold(Pose3) - -@test M == Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) +M = SpecialEuclideanGroup(3; variant=:right) C = 0.2*randn(6) H = coordinates_to_homography(M, C) diff --git a/test/testPose3Pose3NH.jl b/test/testPose3Pose3NH.jl index d886f986..167869c0 100644 --- a/test/testPose3Pose3NH.jl +++ b/test/testPose3Pose3NH.jl @@ -42,7 +42,7 @@ odoCov = deepcopy(initCov) # end # @testset "Adding PriorPose3 to graph..." begin -v1 = addVariable!(fg, :x1, Pose3, N=N) +v1 = addVariable!(fg, :x1, Pose3) initPosePrior = PriorPose3( MvNormal( zeros(6), initCov) ) f1 = addFactor!(fg,[v1], initPosePrior, graphinit=true) @@ -54,7 +54,7 @@ initAll!(fg) # @testset "Ensure variables initialized properly" begin # start with to tight an initialization -muX1 = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), getVal(fg,:x1)) +muX1 = mean(SOnxRn_MetricManifold(3), getVal(fg,:x1)) @test isapprox(submanifold_component(muX1,1), [0,0,0], atol=0.4) @test isapprox(submanifold_component(muX1,2), [1 0 0; 0 1 0; 0 0 1], atol=0.04) @@ -68,7 +68,7 @@ stdX1 = sqrt.(diag(cov(Pose3(), getVal(fg,:x1)))) priorpts = approxConv(fg, :x1f1, :x1) # priorpts = evalFactor(fg, fg.g.vertices[2], 1) -means = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), priorpts) +means = mean(SOnxRn_MetricManifold(3), priorpts) @test sum(map(Int,abs.(submanifold_component(means,1)) .> 0.5)) == 0 @test isapprox(submanifold_component(means,2), [1 0 0; 0 1 0; 0 0 1], atol=0.05) @@ -89,13 +89,13 @@ end ## global fg - +N = fg.solverParams.N X1pts = getVal(fg, :x1) X2pts = approxConv(fg, :x1x2f1, :x2, N=N) # X2pts = evalFactor(fg, fg.g.vertices[6], 3, N=N) X3pts = approxConv(fg, :x2x3f1, :x3) -X2ptsMean = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X2pts) -X3ptsMean = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X3pts) +X2ptsMean = mean(SOnxRn_MetricManifold(3), X2pts) +X3ptsMean = mean(SOnxRn_MetricManifold(3), X3pts) # @show X2ptsMean # @show X3ptsMean @@ -138,8 +138,8 @@ p2t = manikde!(Pose3, X2ptst) # plotKDE([p2t;p2],c=["red";"blue"],dims=[1;2],levels=3) -t1 = mmd(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X1ptst, X1pts, bw=[0.001;]) -t2 = mmd(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X2ptst, X2pts, bw=[0.001;]) +t1 = mmd(SOnxRn_MetricManifold(3), X1ptst, X1pts, bw=[0.001;]) +t2 = mmd(SOnxRn_MetricManifold(3), X2ptst, X2pts, bw=[0.001;]) @test t1 < 0.6 @test t2 < 0.6 diff --git a/test/testSEParameterizations.jl b/test/testSEParameterizations.jl deleted file mode 100644 index aa2373b0..00000000 --- a/test/testSEParameterizations.jl +++ /dev/null @@ -1,199 +0,0 @@ -using LieGroups -using DistributedFactorGraphs -using Distributions -using StaticArrays -using DistributedFactorGraphs: ArrayPartition - -T = ArrayPartition{Float64, Tuple{Matrix{Float64}, Vector{Float64}}} -G = SpecialEuclideanGroup(2; variant=:right) -ε = identity_element(G, T) - -# T-R Coordinates, Chirikjian, p.35 Jr, above 10.76 -function Jr(::Val{:SE2_TR}, Xⁱ::AbstractVector) - @assert length(Xⁱ) == 3 "Xⁱ must be a vector of length 3" - # Xⁱ = [x1, x2, θ] - θ = Xⁱ[3] - return [ - cos(θ) sin(θ) 0; - -sin(θ) cos(θ) 0; - 0 0 1 - ] -end - -# Exponential Coordinates, Chirikjian, p.36 Jr, after 10.76 -function Jr(::Val{:SE2_EXP}, Xⁱ::AbstractVector) - @assert length(Xⁱ) == 3 "Xⁱ must be a vector of length 3" - v1 = Xⁱ[1] - v2 = Xⁱ[2] - α = Xⁱ[3] - if α == 0 - return [1 0 0; - 0 1 0; - 0 0 1.] - end - return [ - (sin(α))/α (1 - cos(α))/α (α*v1 - v2 + v2*cos(α) - v1*sin(α))/α^2; - (cos(α) - 1)/α sin(α)/α (v1 + α*v2 - v1*cos(α) - v2*sin(α))/α^2; - 0 0 1 - ] -end - -function Ad(::typeof(SpecialEuclideanGroup(2; variant=:right)), p) - t = p.x[1] - R = p.x[2] - vcat( - hcat(R, -SA[0 -1; 1 0]*t), - SA[0 0 1] - ) -end - -Ad(G, p) - - -## -# T-R Coordinates -X₀aⁱ = [10, 1, pi/4] -# LieAlgebra(G) is a Fiber? -X₀a = hat(LieAlgebra(G), X₀aⁱ, T) -# base_manifold(G) -> use the default Riemannian metric -p = exp(base_manifold(G), ε, X₀a) -# Exponential Coordinates -X₀b = log(G, p) - -# J = XbJp * pJXa -# J = jacobian_of_log(SE2, p)_wrt_p * jacobain_of_exp(SO2xEuclid2, Xa)_wrt_Xa -# J = J(Log(p), p) * J(exp(Xa), Xa) -J = inv(Jr(Val(:SE2_EXP), vee(LieAlgebra(G), X₀b))) * Jr(Val(:SE2_TR), vee(LieAlgebra(G), X₀a)) - -Σα = [ - 1 0 0; - 0 0.1 0; - 0 0 0.01 -] - -N = 500000 -# generate noise as tangent T-R Coordinates (assuming observation was in T-R) -Xⁱs = rand(MvNormal(Σα), N) - -# map from T-R to exponential tangent representation -function φ(X) # work on better name - # use Riemannian Exponential map to get the points (Chirikjian, p34, (eq. 10.75)) - np = LieGroups.compose(G, p, exp(base_manifold(G), ε, X)) # noisy p - # convert the points back using the group logarithm map - return log(G, p, np) # -> Y -end - -# Convert noisy coordinates in T-R coordinates to Exponential Coordinates at expansion point p -# is this a push-forward? disagreeement from Mateusz/Ronnie... -Yⁱs = map(eachcol(Xⁱs)) do Xⁱ - # vector in a tangent space represented from T-R coordinates, [hybrid tangent representation] - X = hat(LieAlgebra(G), Xⁱ, T) - # map from T-R to exponential tangent representation - Y = φ(X) - # get coordinates in exponential representation - Chirikjian, p35, (eq. 10.76) - Yⁱ = vee(LieAlgebra(G), Y) - return Yⁱ -end - - -Yⁱs = hcat(Yⁱs...) - -fit_Σβ = cov(fit_mle(MvNormal, Yⁱs)) - -# is fit_Σβ now the covariance converted from Σα in T-R coordinates to exponential coordinates at point p on G? - -# is this a valid test? - -# J = XbJp * pJXa -# J = jacobian_of_log(SE2, p)_wrt_p * jacobain_of_exp(SO2xEuclid2, Xa)_wrt_Xa -# J = J(Log(p), p) * J(exp(Xa), Xa) -J = inv(Jr(Val(:SE2_EXP), vee(LieAlgebra(G), Y₀))) * Jr(Val(:SE2_TR), vee(LieAlgebra(G), X₀)); - -# Sola 2018, eq. 55 -prop_Σβ = J * Σα * J' -fit_Σβ -# why do this? comparing what with what? -# What: linear propagate vs nonparametric mapping test of Covariance matrix -# Why: Trying resolve how to do robotics rigid transforms with new LieGroups.jl -isapprox(prop_Σβ, fit_Σβ; atol=1e-3) - - -## ================================================================================= -## -## ================================================================================= - -# set Chirikjian 10.75 = 10.76 -# x1 = (v2*(-1 + cos(α)) + v1*sin(α))/α -# x2 = (v1*( 1 - cos(α)) + v2*sin(α))/α -# θ = α - -function map_TR_to_exp_coords(g) - x1 = g[1] - x2 = g[2] - θ = g[3] - - α = θ - - #FIXME is this correct? - if α == 0 - return [x1, x2, θ] - end - - #FIXME maybe simplify this equations further - v2 = (x1*α - x1*cos(α)*α - x2*sin(α)*α) / (2*(cos(α) - 1)) - v1 = (x2*α - v2*sin(α)) / (1 - cos(α)) - - return [v1, v2, α] -end - -function map_exp_to_TR_coords(g) - v1 = g[1] - v2 = g[2] - α = g[3] - - θ = α - - x1 = (v2*(-1 + cos(α)) + v1*sin(α))/α - x2 = (v1*( 1 - cos(α)) + v2*sin(α))/α - - return [x1, x2, θ] -end - -X₀ⁱ = [10, 1, pi/2] -X₀ = hat(LieAlgebra(G), X₀ⁱ, T) -p = exp(base_manifold(G), ε, X₀) -Y₀ = log(G, p) -Y₀ⁱ = vee(LieAlgebra(G), Y₀) - -Yⁱ = map_TR_to_exp_coords(X₀ⁱ) -isapprox(Y₀ⁱ, Yⁱ) - -Xⁱ = map_exp_to_TR_coords(Yⁱ) -isapprox(X₀ⁱ, Xⁱ) - -N = 50000 -# generate noise as tangent T-R Coordinates in the lie algebra -Xⁱs = rand(MvNormal(Σα), N) -Yⁱs = map(eachcol(Xⁱs)) do Xⁱ - Yⁱ = map_TR_to_exp_coords(Xⁱ) - return Yⁱ -end - -Yⁱs = hcat(Yⁱs...) - -fit_Σβ = cov(fit_mle(MvNormal, Yⁱs)) - -Jfd = FiniteDiff.finite_difference_jacobian(map_TR_to_exp_coords, [0.,0,0]) -prop_Σβ = Jfd * Σα * Jfd' -fit_Σβ - - X = hat(LieAlgebra(G), Xⁱ, T) - q = exp(G, p, X) - vee(LieAlgebra(G), log(G, p, q)) -end - -jac_Xbs = hcat(jac_Xbs...) - - -scatter(Xbs[1:2, :]; axis=(aspect=DataAspect(),), markersize=3) -scatter!(jac_Xbs[1:2, :]; markersize=3) \ No newline at end of file diff --git a/test/testhigherdimroots.jl b/test/testhigherdimroots.jl index 1d376196..e0463d5c 100644 --- a/test/testhigherdimroots.jl +++ b/test/testhigherdimroots.jl @@ -5,6 +5,7 @@ using RoME using Test import IncrementalInference: getSample, getManifold using TransformUtils: Euler +using LieGroups: CircleGroup ## mutable struct RotationTest <: IncrementalInference.AbstractRelativeMinimize @@ -12,7 +13,7 @@ mutable struct RotationTest <: IncrementalInference.AbstractRelativeMinimize end getSample(cfo::CalcFactor{<:RotationTest}) = rand(cfo.factor.z) -getManifold(::RotationTest) = RealCircleGroup() +DFG.getManifold(::RotationTest) = CircleGroup(ℝ) # 3 dimensional line, z = [a b][x y]' + c function (cfo::CalcFactor{<:RotationTest})( meas, diff --git a/test/testpackingconverters.jl b/test/testpackingconverters.jl index bb278303..daaa1e85 100644 --- a/test/testpackingconverters.jl +++ b/test/testpackingconverters.jl @@ -4,18 +4,102 @@ using RoME using Test using DistributedFactorGraphs -import DistributedFactorGraphs: packVariableNodeData, unpackVariableNodeData - +# import DistributedFactorGraphs: packVariableNodeData, unpackVariableNodeData + + +prior_types = [ + PriorPoint2, + PriorPoint3, + PriorPose2, + PriorPose3, + PriorVelPos3, + DynPoint2VelocityPrior, + # DynPose2VelocityPrior, # non standard + # PartialPriorYawPose2, + PriorIMUBias, + # PriorPose3ZRP, # non standard + # PriorInertialPose3, + # PriorPolar, +] + +# FIXME all relative types should be updated to AbstractRelativeObservation +relative_types = [ + # AbstractRelativeObservation + Point2Point2, + Point3Point3, + Pose2Pose2, + Pose3Pose3, + # AbstractRelativeMinimize, + # DynPose2Pose2, + # InertialPose3, + # MixtureFluxPose2Pose2, + # MultipleFeatures2D, + # PolarPolar, + RoME.DynPose2DynPose2, + #AbstractManifoldMinimize, + DynPoint2DynPoint2, + # IMUDeltaFactor, #non standard + InertialDynamic, + # LinearRangeBearingElevation, # non standard + # MutablePose2Pose2Gaussian, + Point2Point2Range, + Point2Point2Velocity, + Pose2Point2, + Pose2Point2Bearing, + # Pose2Point2BearingRange, # non standard + Pose2Point2Range, + Pose3Pose3Rotation, + Pose3Pose3XYYaw, + RoME.Pose3Pose3RotOffset, + RoME.Pose3Pose3Transform, + RoME.Pose3Pose3UnitTrans, + VelAlign, + VelPoint2VelPoint2, + VelPosRotVelPos, + # VelPose2VelPose2, # non standard + # GenericProjection, +] + + +observation_types = [prior_types; relative_types] + +function test_packing(T) + try + d = getDimension(getManifold(T)) > 0 + + Z = MvNormal(zeros(d), diagm(ones(d))) + # create a factor + f = T(Z) + # pack it + packed = pack(f) + # unpack it + unpacked = unpack(packed) + + # check if the original and unpacked are equal + return ==(f, unpacked) + catch e + return e + end +end +@testset "Packing and unpacking of factor observations" begin + for T in observation_types + @testset "Pack/unpack $T" begin + @test test_packing(T) + end + end +end ## +@error "TODO update the rest of the packing tests" +if false @testset "test PriorPoint2" begin ## global prpt2 = PriorPoint2( MvNormal([0.25;0.75], Matrix(Diagonal([1.0;2.0]))) ) -global pprpt2 = convert(PackedPriorPoint2, prpt2) -global uprpt2 = convert(PriorPoint2, pprpt2) +global pprpt2 = pack(prpt2) +global uprpt2 = unpack(pprpt2) @test norm(prpt2.Z.μ - uprpt2.Z.μ) < 1e-8 @@ -53,12 +137,12 @@ global fg ## -dd = convert(PackedPriorPose2, ipp) -upd = convert(RoME.PriorPose2, dd) +dd = pack(ipp) +upd = unpack(dd) @test RoME.compare(ipp, upd) -packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPriorPose2}, DFG.getSolverData(f1)) +packeddata = pack(getObservation(f1)) unpackeddata = reconstFactorData(fg, getVariableOrder(f1), IIF.FunctionNodeData{IIF.CommonConvWrapper{RoME.PriorPose2}}, packeddata); # unpackeddata = convert(IIF.FunctionNodeData{IIF.CommonConvWrapper{RoME.PriorPose2}}, packeddata) @@ -74,7 +158,7 @@ upv1data = unpackVariableNodeData(packedv1data) # upv1data = convert(IncrementalInference.VariableNodeData, packedv1data) @test compareAll(DFG.getSolverData(v1), upv1data, skip=[:variableType;:val]) -@test all( isapprox.(DFG.getSolverData(v1).val, upv1data.val) ) +@test all( isapprox.(DFG.getState(v1).val, upv1data.val) ) @test compareFields(DFG.getSolverData(v1).variableType, upv1data.variableType) ## @@ -84,8 +168,8 @@ end @testset "test conversions of Pose2Pose2" begin ## -global dd = convert(PackedPose2Pose2, ppc) -global upd = convert(RoME.Pose2Pose2, dd) +global dd = pack(ppc) +global upd = unpack(dd) # TODO -- fix ambiguity in compare function @test RoME.compare(ppc, upd) @@ -112,11 +196,8 @@ global ppbr = Pose2Point2BearingRange( Normal(50, 0.5) ) global f3 = addFactor!(fg, [:x2;:l1], ppbr) -global dd = convert(PackedPose2Point2BearingRange, ppbr) -global upd = convert( - RoME.Pose2Point2BearingRange, - dd - ) +global dd = pack(ppbr) +global upd = unpack(dd) global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose2Point2BearingRange}, DFG.getSolverData(f3)) @@ -151,8 +232,8 @@ global f1 = addFactor!(fg,[v1], ipp) ## -global dd = convert(PackedPriorPose3, ipp) -global upd = convert(RoME.PriorPose3, dd) +global dd = pack(ipp) +global upd = unpack(dd) # @test TransformUtils.compare(ipp.Zi, upd.Zi) @test norm(ipp.Z.μ - upd.Z.μ) < 1e-10 @@ -203,8 +284,8 @@ f2 = addFactor!(fg, [:x1, :x2], pp3) @testset "test conversions of Pose3Pose3" begin -global dd = convert(PackedPose3Pose3, pp3) -global upd = convert(RoME.Pose3Pose3, dd) +global dd = pack(pp3) +global upd = unpack(dd) @test norm(pp3.Z.μ - upd.Z.μ) < 1e-10 @@ -232,8 +313,8 @@ global f3 = addFactor!(fg,[:x1;:x2],odoc, nullhypo=0.5) global prpz = PriorPose3ZRP(MvNormal([0.0;0.5],0.1*diagm([1.0;1])),Normal(3.0,0.5)) -global pprpz = convert(PackedPriorPose3ZRP, prpz) -global unp = convert(PriorPose3ZRP, pprpz) +global pprpz = pack(prpz) +global unp = unpack(pprpz) @test RoME.compare(prpz, unp) @@ -242,11 +323,10 @@ end @testset "test conversions of PartialPose3XYYaw" begin -global xyy = Pose3Pose3XYYaw( - MvNormal( [1.0;2.0;0.5], 0.1*diagm([1.0;1;1]) )) +global xyy = Pose3Pose3XYYaw(MvNormal( [1.0;2.0;0.5], 0.1*diagm([1.0;1;1]) )) -global pxyy = convert(PackedPose3Pose3XYYaw, xyy) -global unp = convert(Pose3Pose3XYYaw, pxyy) +global pxyy = pack(xyy) +global unp = unpack(pxyy) @test RoME.compare(xyy, unp) end @@ -257,11 +337,11 @@ end p3rot = Pose3Pose3Rotation(MvNormal( [0.1;0.2;0.3], 0.1*diagm([1.0;1;1]) )) -pac = convert(PackedPose3Pose3Rotation, p3rot) -unp = convert(Pose3Pose3Rotation, p3rot) +pac = pack(p3rot) +unp = unpack(pac) @test RoME.compare(p3rot, unp) end - +end # diff --git a/test/threeDimLinearProductTest.jl b/test/threeDimLinearProductTest.jl index 9bd524bd..1a41736c 100644 --- a/test/threeDimLinearProductTest.jl +++ b/test/threeDimLinearProductTest.jl @@ -4,6 +4,7 @@ using TransformUtils using Statistics using Test using StaticArrays +using LieGroups using Manifolds @@ -104,7 +105,7 @@ T = submanifold_component(muX1,1) @test sum(map(Int,abs.(T) .< 0.5)) == 3 Rc = submanifold_component(muX1,2) -@test isapprox(SpecialOrthogonal(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) +@test isapprox(SpecialOrthogonalGroup(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) coX1 = IIF.getCoordinates.(Pose3, getVal(fg,:x1)) @show stdX1 = Statistics.std(coX1) @@ -136,9 +137,7 @@ end ## N -odo = SE3([10;0;0], Quaternion(0)) -# pts0X2 = projectParticles(getVal(fg,:x1), MvNormal(veeEuler(odo), odoCov) ) -odoconstr = Pose3Pose3( MvNormal(veeEuler(odo), odoCov) ) +odoconstr = Pose3Pose3( MvNormal([10.0, 0, 0, 0, 0, 0], odoCov) ) v2 = addVariable!(fg,:x2, Pose3, N=N) # pts0X2 addFactor!(fg,[:x1;:x2],odoconstr, inflation=0.1) # @test !isInitialized(fg, :x2) @@ -146,25 +145,43 @@ addFactor!(fg,[:x1;:x2],odoconstr, inflation=0.1) ## test opposites -# should be all zero -p = deepcopy(ϵ) -# q = deepcopy(ϵ) -# submanifold_component(q,1)[1] = 10.0 -q = ArrayPartition(SA[10.0, 0.0, 0.0], SA[1.0 0.0 0.0; 0.0 1.0 0.0; 0.0 0.0 1.0]) -X = Manifolds.hat(M, ϵ, [10.,0,0,0,0,0]) -res = calcFactorResidual(fg, :x1x2f1, X, p, q) -@test norm(res) < 1e-10 - -# trivial fail case -# res = calcFactorResidual(fg, :x1x2f1, [10;0;0;0;0;0.0], zeros(6), [10;0;0;pi;pi;pi]) -@warn "suppressing trivial Pose3 fail case until RoME.jl #244 has been completed." -#TODO is this the case for the warn? -p = deepcopy(ϵ) -q = IIF.getPoint(Pose3, [10;0;0;pi;pi;pi]) -X = Manifolds.hat(M, ϵ, [10.,0,0,pi,pi,pi]) +#test factor :x1x2f1 residuals (zero) at a few points +G = getManifold(getFactor(fg, :x1x2f1)) + +points = [ + deepcopy(ϵ), + exp(G, hat(G, ϵ, [10.;0;0;normalize([1,1,1])*2pi/3])) +] + +meas_coords = [ + SA[10.,0,0,0,0,0], + SA[10.,0,0,0,0,pi-0.01], + SVector{6}([10.;0;0;normalize([1,1,1])*(pi-1e-3)]), +] + +for p in points, c in meas_coords + X = hat(LieAlgebra(G), c, typeof(p)) + q = exp(G, p, X) + res = calcFactorResidual(fg, :x1x2f1, X, p, q) + @test norm(res) < 1e-9 +end +p = points[2] +# the measurement +X = hat(LieAlgebra(G), SA[1, -2, 3, 0.1, -0.2, 0.3], typeof(p)) +# the state +X̂ = hat(LieAlgebra(G), SA[1.1, -2.1, 2.9, 0.11, -0.21, 0.29], typeof(p)) +q = exp(G, p, X̂) res = calcFactorResidual(fg, :x1x2f1, X, p, q) -@test norm(res) < 1e-10 +@test all(isapprox.(res, vee(LieAlgebra(G), X - X̂))) + +#FIXME add injection radius assertions to factor observations (measurements) +# X here is not within the injectivity radius of G, therefore the log will not be valid and the results wrong +# p = deepcopy(ϵ) +# q = IIF.getPoint(Pose3, [10;0;0;pi;pi;pi]) +# X = Manifolds.hat(G, ϵ, [10.,0,0,pi,pi,pi]) +# res = calcFactorResidual(fg, :x1x2f1, X, p, q) +# @test norm(res) < 1e-10 ## test following introduction of inflation, see IIF #1051 @@ -207,13 +224,13 @@ T = submanifold_component(mu,1) # slightly lax bound @test isapprox(T, [0,0,0], atol=0.3) # 0.6 Rc = submanifold_component(mu,2) -@test isapprox(SpecialOrthogonal(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) +@test isapprox(SpecialOrthogonalGroup(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) mu = mean(M, getVal(fg,:x2)) T = submanifold_component(mu,1) @test isapprox(T, [10,0,0], atol=1.0) Rc = submanifold_component(mu,2) -@test isapprox(SpecialOrthogonal(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) +@test isapprox(SpecialOrthogonalGroup(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) end @@ -226,13 +243,13 @@ mu = mean(M, getVal(fg,:x1)) T = submanifold_component(mu,1) @test isapprox(T, [0,0,0], atol=1.5) Rc = submanifold_component(mu,2) -@test isapprox(SpecialOrthogonal(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) +@test isapprox(SpecialOrthogonalGroup(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) mu = mean(M, getVal(fg,:x2)) T = submanifold_component(mu,1) @test isapprox(T, [10,0,0], atol=1.5) Rc = submanifold_component(mu,2) -@test isapprox(SpecialOrthogonal(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) +@test isapprox(SpecialOrthogonalGroup(3), Rc, [1 0 0; 0 1 0; 0 0 1], atol=0.25) ## end From e5f02e46573239eb84a857ce019d25ab6b173baa Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Wed, 27 Aug 2025 12:27:22 +0200 Subject: [PATCH 15/18] Apply suggestions from code review Co-authored-by: Dehann Fourie <6412556+dehann@users.noreply.github.com> --- RoMETypes/Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/RoMETypes/Project.toml b/RoMETypes/Project.toml index 40b99f5c..b0ba3ccc 100644 --- a/RoMETypes/Project.toml +++ b/RoMETypes/Project.toml @@ -1,6 +1,6 @@ name = "RoMETypes" uuid = "298dcef3-e3bd-403a-988f-2dd5ca311a3f" -authors = ["Johannes Terblanche "] +authors = ["NavAbility(TM) Contributors "] version = "0.1.0" [deps] From b02e7e5253b645892b4995b01c9fde25616ce1f1 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 29 Aug 2025 12:50:08 +0200 Subject: [PATCH 16/18] add IIFTypes --- .github/workflows/ci.yml | 5 +++-- Project.toml | 6 ++++-- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3ac3e82c..b3195f4f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -77,8 +77,9 @@ jobs: - name: Run tests on Upstream Dev run: | - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="IncrementalInference",rev="develop"));' + julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInferenceTypes"));' + julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInference"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.test("RoME"; coverage=false)' + julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.test("RoME"; coverage=true)' shell: bash diff --git a/Project.toml b/Project.toml index 2b176e61..aeddf19e 100644 --- a/Project.toml +++ b/Project.toml @@ -16,6 +16,7 @@ DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" FileIO = "5789e2e9-d7fb-5bc7-8068-2c6fae9b9549" ImageCore = "a09fc81d-aa75-5fe9-8630-4744c3626534" IncrementalInference = "904591bb-b899-562f-9e6f-b8df64c7d480" +IncrementalInferenceTypes = "9808408f-4dbc-47e4-913c-6068b950e289" Interpolations = "a98d9a8b-a2ab-59e6-89dd-64a1c18fca59" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" LieGroups = "6774de46-80ba-43f8-ba42-e41071ccfc5f" @@ -63,7 +64,8 @@ FileIO = "1" Flux = "0.14, 0.15, 0.16" ImageCore = "0.9, 0.10" ImageIO = "0.6" -IncrementalInference = "0.36" +IncrementalInference = "0.36.0" +IncrementalInferenceTypes = "0.1.0" Interpolations = "0.14, 0.15" KernelDensityEstimate = "0.5.1, 0.6" LieGroups = "0.1.1" @@ -93,4 +95,4 @@ JSON3 = "0f8b85d8-7281-11e9-16c2-39a750bddbf1" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [targets] -test = ["CameraModels", "DifferentialEquations", "Flux", "ImageIO", "JSON3", "Test"] +test = ["CameraModels", "DifferentialEquations", "Flux", "JSON3", "Test"] From 802d94f250779c9cb691771bde1f11ffebab310a Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 29 Aug 2025 13:00:18 +0200 Subject: [PATCH 17/18] change order of upstream dev packages --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b3195f4f..5e6d459f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -77,9 +77,9 @@ jobs: - name: Run tests on Upstream Dev run: | + julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"));' + julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInferenceTypes"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInference"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.test("RoME"; coverage=true)' shell: bash From f12f2740242cb44d93ec9e86534f94f95b930b39 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Fri, 29 Aug 2025 13:14:24 +0200 Subject: [PATCH 18/18] rewrite CI Upstream Dev --- .github/workflows/ci.yml | 43 ++++++++++++++++------------------------ 1 file changed, 17 insertions(+), 26 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5e6d459f..2ed0aac9 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -46,40 +46,31 @@ jobs: with: file: lcov.info - test-masters: + upstream-dev: #if: github.ref != 'refs/heads/release**' name: Upstream Dev runs-on: ubuntu-latest env: JULIA_PKG_SERVER: "" steps: - - uses: actions/checkout@v2 - + - uses: actions/checkout@v4 - uses: julia-actions/setup-julia@v2 with: - version: '1.10' + version: '1' arch: x64 - - - uses: actions/cache@v4 + - uses: julia-actions/cache@v2 + - name: Dev dependencies + shell: julia --project=monorepo {0} + run: | + using Pkg; + Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop")) + Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop")) + Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInferenceTypes")) + Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInference")) + Pkg.develop(PackageSpec(path=".")) + - name: Run tests env: - cache-name: cache-artifacts - with: - path: ~/.julia/artifacts - key: ${{ runner.os }}-test-${{ env.cache-name }}-${{ hashFiles('**/Project.toml') }} - restore-keys: | - ${{ runner.os }}-test-${{ env.cache-name }}- - ${{ runner.os }}-test- - ${{ runner.os }}- + IIF_TEST_GROUP: 'basic_functional_group' + run: > + julia --color=yes --project=monorepo -e 'using Pkg; Pkg.test("RoME")' - - run: | - git config --global user.name Tester - git config --global user.email te@st.er - - - name: Run tests on Upstream Dev - run: | - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="develop"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="develop"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInferenceTypes"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(url="https://github.com/JuliaRobotics/IncrementalInference.jl",rev="develop",subdir="IncrementalInference"));' - julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.test("RoME"; coverage=true)' - shell: bash