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

Author: Affie

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"

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

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
 
 
 

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

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

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,

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,

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];

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()
 

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]))

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
 

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

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))

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)

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)

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])
+)
 
 
 

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

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])))

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]

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

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)

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 )