From 455edaadbdff90300fd2245c909e27ca267d2024 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche <6612981+Affie@users.noreply.github.com> Date: Thu, 3 Oct 2024 12:23:26 +0200 Subject: [PATCH] Updates for manifolds v0.10 (#754) * Updates for manifolds v0.10 * fix getManifold(::DynPose2DynPose2) * add docstring to IMUDeltaGroup + references * Fix imu factor test, the factor looks correct --- Project.toml | 5 ++-- ext/RoMECameraModelsExt.jl | 1 + src/Deprecated.jl | 11 ++++++--- src/RoME.jl | 5 ++-- src/factors/BearingRange2D.jl | 2 +- src/factors/DynPose2D.jl | 2 +- src/factors/Inertial/IMUDeltaFactor.jl | 34 +++++++++++++++----------- src/factors/PartialPose3.jl | 4 +-- src/factors/Pose2D.jl | 2 +- src/factors/Pose2Point2.jl | 2 +- src/factors/PriorPose2.jl | 4 +-- src/factors/Range2D.jl | 2 +- src/services/AdditionalUtils.jl | 2 +- src/services/FixmeManifolds.jl | 10 ++++---- src/services/ManifoldUtils.jl | 4 +-- src/variables/VariableTypes.jl | 19 ++++++++++---- test/inertial/testIMUDeltaFactor.jl | 11 ++++++--- test/testBearing2D.jl | 2 +- test/testBearingRange2D.jl | 2 +- test/testDidsonFunctions.jl | 2 +- test/testParametric.jl | 2 +- test/testParametricCovariances.jl | 2 +- test/testPartialPose3.jl | 4 +-- test/testPose3.jl | 2 +- test/testPose3Pose3NH.jl | 12 ++++----- 25 files changed, 87 insertions(+), 61 deletions(-) diff --git a/Project.toml b/Project.toml index 818a7c8a..c72d007d 100644 --- a/Project.toml +++ b/Project.toml @@ -2,7 +2,7 @@ name = "RoME" uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.24.5" +version = "0.24.6" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" @@ -26,6 +26,7 @@ OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d" PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" +RecursiveArrayTools = "731186ca-8d62-57ce-b412-fbd966d074cd" Reexport = "189a3867-3050-52da-a836-e630ba90ab69" Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" @@ -65,7 +66,7 @@ IncrementalInference = "0.35" Interpolations = "0.14, 0.15" KernelDensityEstimate = "0.5.1, 0.6" LinearAlgebra = "1.10" -Manifolds = "0.9" +Manifolds = "0.10.1" ManifoldsBase = "0.15" Optim = "0.22, 1" OrderedCollections = "1" diff --git a/ext/RoMECameraModelsExt.jl b/ext/RoMECameraModelsExt.jl index bd59b70c..e7001a56 100644 --- a/ext/RoMECameraModelsExt.jl +++ b/ext/RoMECameraModelsExt.jl @@ -7,6 +7,7 @@ using StaticArrays using Manifolds using DocStringExtensions using Optim +using RecursiveArrayTools: ArrayPartition import Base: convert import IncrementalInference: AbstractDFG, getFactorType, getVariable, getSolverData, CalcFactor, ls diff --git a/src/Deprecated.jl b/src/Deprecated.jl index a31060ab..1daae1f0 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -64,8 +64,13 @@ Base.convert( 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))))}) = (: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{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{Point2Point2}) = AMP.Euclid2 @@ -77,8 +82,8 @@ Base.convert(::Type{<:Tuple}, ::IIF.InstanceType{typeof(Manifolds.ProductGroup(P # 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}) = SE2E2_Manifold -Base.convert(::Type{<:ManifoldsBase.AbstractManifold}, ::IIF.InstanceType{VelPose2VelPose2}) = SE2E2_Manifold +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/RoME.jl b/src/RoME.jl index 9c8677c1..1ae4c77c 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -20,11 +20,12 @@ using DocStringExtensions, DistributedFactorGraphs, TensorCast, - ManifoldsBase + ManifoldsBase, + Manifolds using StaticArrays 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 diff --git a/src/factors/BearingRange2D.jl b/src/factors/BearingRange2D.jl index d7323959..02e26f66 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))) +getManifold(::IIF.InstanceType{<:Pose2Point2BearingRange}) = ProductGroup(ProductManifold(SpecialOrthogonal(2), TranslationGroup(1)), LeftInvariantRepresentation()) function getSample(cfo::CalcFactor{<:Pose2Point2BearingRange}) # defaults, TODO better reuse diff --git a/src/factors/DynPose2D.jl b/src/factors/DynPose2D.jl index f8eab8a2..9f8777f3 100644 --- a/src/factors/DynPose2D.jl +++ b/src/factors/DynPose2D.jl @@ -147,7 +147,7 @@ end preambleCache(::AbstractDFG, ::AbstractVector{<:DFGVariable}, ::DynPose2DynPose2) = zeros(5) # FIXME ON FIRE, must update to new Manifolds style factors -getManifold(::DynPose2DynPose2) = SE2E2_Manifold # not fully impl manifold yet +getManifold(::DynPose2DynPose2) = getManifold(DynPose2) # FIXME, should produce tangents, not coordinates. getSample(cf::CalcFactor{<:DynPose2DynPose2}) = rand(cf.factor.Z) function (cf::CalcFactor{<:DynPose2DynPose2})(meas, diff --git a/src/factors/Inertial/IMUDeltaFactor.jl b/src/factors/Inertial/IMUDeltaFactor.jl index 07549159..4078dc11 100644 --- a/src/factors/Inertial/IMUDeltaFactor.jl +++ b/src/factors/Inertial/IMUDeltaFactor.jl @@ -10,20 +10,28 @@ struct IMUDeltaManifold <: AbstractManifold{ℝ} end # NOTE Manifold in not defined as a ProductManifold since we do not use the product metric. #701 # also see related SE₂(3) +""" + IMUDeltaGroup + +#TODO SpecialGalileanGroup(3) +References: +- https://hal.science/hal-02183498/document +- TODO new reference: https://arxiv.org/pdf/2312.07555 +Affine representation +Δ = [ΔR Δv Δp; + 0 1 Δt; + 0 0 1] ⊂ ℝ⁵ˣ⁵ + +ArrayPartition representation (TODO maybe swop order to [Δp; Δv; ΔR; Δt]) +Δ = [ΔR; Δv; Δp; Δt] +""" const IMUDeltaGroup = GroupManifold{ℝ, IMUDeltaManifold, MultiplicationOperation} -IMUDeltaGroup() = GroupManifold(IMUDeltaManifold(), MultiplicationOperation()) +IMUDeltaGroup() = GroupManifold(IMUDeltaManifold(), MultiplicationOperation(), LeftInvariantRepresentation()) Manifolds.manifold_dimension(::IMUDeltaManifold) = 9 -# Affine representation -# Δ = [ΔR Δv Δp; -# 0 1 Δt; -# 0 0 1] ⊂ \R^5x5 - -# ArrayPartition representation (TODO maybe swop order to [Δp; Δv; ΔR; Δt]) -# Δ = [ΔR; Δv; Δp; Δt] function Manifolds.identity_element(M::IMUDeltaGroup) # was #SMatrix{5,5,Float64}(I) ArrayPartition( @@ -387,10 +395,8 @@ function (cf::CalcFactor{<:IMUDeltaFactor})( q_vel, b::SVector{6,T} = zeros(SVector{6,Float64}) ) where T <: Real - p_t = Dates.value(cf.cache.timestams[1])*1e-9 - q_t = Dates.value(cf.cache.timestams[2])*1e-9 - p = ArrayPartition(p_SE3.x[2], p_vel, p_SE3.x[1], p_t) - q = ArrayPartition(q_SE3.x[2], q_vel, q_SE3.x[1], q_t) + 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) end @@ -466,7 +472,7 @@ function IMUDeltaFactor( S = Σ[1:9,1:9] ch = check_point(SM, S; atol = 1e-9) if !isnothing(ch) - @warn "IMU Covar check" ch + @warn "IMU Covar check" ch maxlog=1 S = (S + S') / 2 S = S + diagm((diag(S) .== 0)*1e-15) ch = check_point(SM, S) @@ -480,7 +486,7 @@ function IMUDeltaFactor( Δt, Δ, SMatrix{10,10,Float64}(Σ), - J_b, + SMatrix{10,6,Float64}(J_b), SA[a_b...; ω_b...], IMUMeasurements( accels, diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index b9b0cdcc..2c5855d4 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -109,13 +109,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) +getManifold(::Pose3Pose3XYYaw) = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ## 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) + M = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) rx = normalize(view(wTp.x[2],1:2, 1)) R = SA[rx[1] -rx[2]; diff --git a/src/factors/Pose2D.jl b/src/factors/Pose2D.jl index ccbc2bd6..7d2ee840 100644 --- a/src/factors/Pose2D.jl +++ b/src/factors/Pose2D.jl @@ -31,7 +31,7 @@ Base.@kwdef struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldM Z::T = MvNormal(Diagonal([1.0; 1.0; 1.0])) end -DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2) +DFG.getManifold(::InstanceType{Pose2Pose2}) = Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()) Pose2Pose2(::UniformScaling) = Pose2Pose2() diff --git a/src/factors/Pose2Point2.jl b/src/factors/Pose2Point2.jl index 3d0d9277..d0d041e6 100644 --- a/src/factors/Pose2Point2.jl +++ b/src/factors/Pose2Point2.jl @@ -24,7 +24,7 @@ function (cfo::CalcFactor{<:Pose2Point2})(p_Xpq, w_T_p, w_Tl_q ) # - M = SpecialEuclidean(2) + M = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) 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])) diff --git a/src/factors/PriorPose2.jl b/src/factors/PriorPose2.jl index f4e1be27..1bd1c213 100644 --- a/src/factors/PriorPose2.jl +++ b/src/factors/PriorPose2.jl @@ -16,11 +16,11 @@ end DFG.getManifold(::InstanceType{PriorPose2}) = getManifold(Pose2) # SpecialEuclidean(2) -@inline function _vee(::typeof(SpecialEuclidean(2)), X::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real +@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)), 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 +@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 diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl index a5e28aab..ebfb526d 100644 --- a/src/factors/Range2D.jl +++ b/src/factors/Range2D.jl @@ -48,7 +48,7 @@ Pose2Point2Range(Z::T) where {T <: IIF.SamplableBelief} = Pose2Point2Range(;Z) getManifold(::Pose2Point2Range) = TranslationGroup(1) -function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi::Manifolds.ArrayPartition, lm) +function (cfo::CalcFactor{<:Pose2Point2Range})(rho, xi::ArrayPartition, lm) # Basically `EuclidDistance` return rho .- norm(lm .- xi.x[1]) end diff --git a/src/services/AdditionalUtils.jl b/src/services/AdditionalUtils.jl index 571e359d..024ef0be 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) + M = SpecialEuclidean(len; vectors=HybridTangentRepresentation()) 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 e31852ce..963dcf2c 100644 --- a/src/services/FixmeManifolds.jl +++ b/src/services/FixmeManifolds.jl @@ -21,7 +21,7 @@ MB.manifold_dimension(::_SE2E2) = 5 function AMP.makePointFromCoords(::typeof(SE2E2_Manifold), p::AbstractVector{<:Real}) α = p[3] - Manifolds.ArrayPartition(([p[1], p[2]]), [cos(α) -sin(α); sin(α) cos(α)], ([p[4], p[5]])) + ArrayPartition(([p[1], p[2]]), [cos(α) -sin(α); sin(α) cos(α)], ([p[4], p[5]])) end function AMP.getPoints(mkd::ManifoldKernelDensity{M}) where {M <: typeof(SE2E2_Manifold)} @@ -30,11 +30,11 @@ function AMP.getPoints(mkd::ManifoldKernelDensity{M}) where {M <: typeof(SE2E2_M end function Statistics.mean(::typeof(SE2E2_Manifold), pts::AbstractVector) - se2_ = (d->Manifolds.ArrayPartition(submanifold_component(d, 1), submanifold_component(d, 2))).(pts) - mse2 = mean(Manifolds.SpecialEuclidean(2), se2_) - e2_ = (d->Manifolds.ArrayPartition(submanifold_component(d, 3))).(pts) + se2_ = (d->ArrayPartition(submanifold_component(d, 1), submanifold_component(d, 2))).(pts) + mse2 = mean(Manifolds.SpecialEuclidean(2; vectors=HybridTangentRepresentation()), se2_) + e2_ = (d->ArrayPartition(submanifold_component(d, 3))).(pts) me2 = mean(Euclidean(2), e2_) - Manifolds.ArrayPartition(submanifold_component(mse2, 1), submanifold_component(mse2, 2), submanifold_component(me2, 1)) + ArrayPartition(submanifold_component(mse2, 1), submanifold_component(mse2, 2), submanifold_component(me2, 1)) end # AMP._makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(SE2E2_Manifold)} = coords(M, prr) diff --git a/src/services/ManifoldUtils.jl b/src/services/ManifoldUtils.jl index 58f8444b..05ed3bc2 100644 --- a/src/services/ManifoldUtils.jl +++ b/src/services/ManifoldUtils.jl @@ -6,7 +6,7 @@ function homography_to_coordinates( - M::typeof(SpecialEuclidean(3)), + M::typeof(SpecialEuclidean(3; vectors=HybridTangentRepresentation())), pHq::AbstractMatrix{<:Real} ) Mr = M.manifold[2] @@ -15,7 +15,7 @@ function homography_to_coordinates( end function coordinates_to_homography( - M::typeof(SpecialEuclidean(3)), + M::typeof(SpecialEuclidean(3; vectors=HybridTangentRepresentation())), pCq::AbstractVector ) e0 = Identity(M) diff --git a/src/variables/VariableTypes.jl b/src/variables/VariableTypes.jl index 70437c3b..84b02d60 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) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) +@defVariable Pose2 SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]) """ $(TYPEDEF) @@ -44,7 +44,7 @@ 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) ArrayPartition(SA[0;0;0.0],SA[1 0 0; 0 1 0; 0 0 1.0]) +@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] @@ -57,7 +57,8 @@ Future: SpecialOrthogonal(3), TranslationGroup(3), TranslationGroup(3) - ) + ), + LeftInvariantRepresentation() ), ArrayPartition( SA[1 0 0; 0 1 0; 0 0 1.0], @@ -74,7 +75,8 @@ Future: ProductManifold( TranslationGroup(3), TranslationGroup(3) - ) + ), + LeftInvariantRepresentation() ), ArrayPartition( SA[0; 0; 0.0], @@ -104,7 +106,14 @@ Note - The `SE2E2_Manifold` definition used currently is a hack to simplify the transition to Manifolds.jl, see #244 - Replaced `SE2E2_Manifold` hack with `ProductManifold(SpecialEuclidean(2), TranslationGroup(2))`, confirm if it is correct. """ -@defVariable DynPose2 Manifolds.ProductGroup(ProductManifold(SpecialEuclidean(2), TranslationGroup(2))) ArrayPartition(ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]),SA[0;0.0]) +@defVariable( + DynPose2, + Manifolds.ProductGroup( + ProductManifold(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), TranslationGroup(2)), + LeftInvariantRepresentation() + ), + ArrayPartition(ArrayPartition(SA[0;0.0],SA[1 0; 0 1.0]),SA[0;0.0]) +) diff --git a/test/inertial/testIMUDeltaFactor.jl b/test/inertial/testIMUDeltaFactor.jl index 1a41608f..5760264a 100644 --- a/test/inertial/testIMUDeltaFactor.jl +++ b/test/inertial/testIMUDeltaFactor.jl @@ -141,7 +141,7 @@ 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) +M_SE3 = SpecialEuclidean(3; vectors=HybridTangentRepresentation()) X_SE3 = hat(M_SE3, getPointIdentity(M_SE3), SA[1,0,0, 0,0,θ] * 0.1) p_SE3 = exp_lie(M_SE3, X_SE3) @test isapprox(p.x[3], p_SE3.x[1]) @@ -172,11 +172,14 @@ fac = RoME.IMUDeltaFactor( # Rotation part M_SO3 = SpecialOrthogonal(3) ΔR = identity_element(M_SO3) -for g in imu.gyros[1:end-1] +#NOTE internally integration is done from 2:end +# 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[2:end] exp!(M_SO3, ΔR, ΔR, hat(M_SO3, Identity(M_SO3), g*dt)) end -#TODO I would have expected these 2 to be exactly the same -@test isapprox(M_SO3, fac.Δ.x[1], ΔR; atol=1e-5) +@test isapprox(M_SO3, fac.Δ.x[1], ΔR) # Velocity part @test isapprox(fac.Δ.x[2], [0,0,8.81], atol=1e-3) # after 1 second at 9.81 m/s # position part diff --git a/test/testBearing2D.jl b/test/testBearing2D.jl index d047d9be..694ee7ff 100644 --- a/test/testBearing2D.jl +++ b/test/testBearing2D.jl @@ -10,7 +10,7 @@ using Manifolds: hat @testset "Testing Bearing2D factor" begin ## -M = SpecialEuclidean(2) +M = SpecialEuclidean(2; vectors=HybridTangentRepresentation()) ϵ = getPointIdentity(M) ps = [exp(M, ϵ, hat(M, ϵ, [0.,0,0]))] push!(ps, exp(M, ϵ, hat(M, ϵ, [5.,0,0]))) diff --git a/test/testBearingRange2D.jl b/test/testBearingRange2D.jl index 1577d7e4..086bcc3e 100644 --- a/test/testBearingRange2D.jl +++ b/test/testBearingRange2D.jl @@ -340,7 +340,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), _pts) +p_μ = mean(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), _pts) _pts = IIF.getCoordinates.(Pose2, _pts) @cast pts[j,i] := _pts[i][j] diff --git a/test/testDidsonFunctions.jl b/test/testDidsonFunctions.jl index 3078f8c9..a19df525 100644 --- a/test/testDidsonFunctions.jl +++ b/test/testDidsonFunctions.jl @@ -51,7 +51,7 @@ addFactor!(fg, [:x0;:x1], meas, graphinit=false) pts = approxConv(fg, :x0x1f1, :x0) -p2 = manikde!(SpecialEuclidean(3), pts); +p2 = manikde!(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), pts); end diff --git a/test/testParametric.jl b/test/testParametric.jl index c358cdd0..182bfd22 100644 --- a/test/testParametric.jl +++ b/test/testParametric.jl @@ -171,7 +171,7 @@ IIF.initParametricFrom!(fg) PM, varLabels, r, Σ = IIF.solveGraphParametric(fg) #autodiff=:finite) -@test isapprox(SpecialEuclidean(2), r[1], ArrayPartition([2, 0.], [0 -1; 1 0.]), atol = 1e-3) +@test isapprox(SpecialEuclidean(2; vectors=HybridTangentRepresentation()), 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/testParametricCovariances.jl b/test/testParametricCovariances.jl index e145b6f1..a6fb84e1 100644 --- a/test/testParametricCovariances.jl +++ b/test/testParametricCovariances.jl @@ -46,7 +46,7 @@ addFactor!(fg, [:x0,:x1], Pose2Pose2(MvNormal([0.9,0,0], diagm([sqrt(0.03),0.1,0 ## IIF.autoinitParametric!(fg) -IIF.solveGraph!(fg) +# IIF.solveGraph!(fg) @test isapprox( [0;0;0.], getPPESuggested(fg, :x0, :parametric); atol=1e-4 ) @test isapprox( [1.05;0;0], getPPESuggested(fg, :x1, :parametric); atol=1e-4 ) diff --git a/test/testPartialPose3.jl b/test/testPartialPose3.jl index 31a038f1..ff7c7ea4 100644 --- a/test/testPartialPose3.jl +++ b/test/testPartialPose3.jl @@ -35,7 +35,7 @@ end fg = initfg() -M=SpecialEuclidean(3) +M=SpecialEuclidean(3; vectors=HybridTangentRepresentation()) N = 100 fg.solverParams.N = N fg.solverParams.graphinit = false @@ -486,7 +486,7 @@ end ## -M = SpecialEuclidean(3) +M = SpecialEuclidean(3; vectors=HybridTangentRepresentation()) mpts = getPoints(fg[1], :x4) mu_fg1 = mean(M, mpts) diff --git a/test/testPose3.jl b/test/testPose3.jl index 131a69ac..bf8f74ec 100644 --- a/test/testPose3.jl +++ b/test/testPose3.jl @@ -11,7 +11,7 @@ using StaticArrays M = getManifold(Pose3) -@test M == Manifolds.SpecialEuclidean(3) +@test M == Manifolds.SpecialEuclidean(3; vectors=HybridTangentRepresentation()) C = 0.2*randn(6) H = coordinates_to_homography(M, C) diff --git a/test/testPose3Pose3NH.jl b/test/testPose3Pose3NH.jl index e4043513..d886f986 100644 --- a/test/testPose3Pose3NH.jl +++ b/test/testPose3Pose3NH.jl @@ -54,7 +54,7 @@ initAll!(fg) # @testset "Ensure variables initialized properly" begin # start with to tight an initialization -muX1 = mean(SpecialEuclidean(3), getVal(fg,:x1)) +muX1 = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), 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), priorpts) +means = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), 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) @@ -94,8 +94,8 @@ 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), X2pts) -X3ptsMean = mean(SpecialEuclidean(3), X3pts) +X2ptsMean = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X2pts) +X3ptsMean = mean(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), 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), X1ptst, X1pts, bw=[0.001;]) -t2 = mmd(SpecialEuclidean(3), X2ptst, X2pts, bw=[0.001;]) +t1 = mmd(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X1ptst, X1pts, bw=[0.001;]) +t2 = mmd(SpecialEuclidean(3; vectors=HybridTangentRepresentation()), X2ptst, X2pts, bw=[0.001;]) @test t1 < 0.6 @test t2 < 0.6