diff --git a/src/ParametricUtils.jl b/src/ParametricUtils.jl index b60d7b7b..31fea151 100644 --- a/src/ParametricUtils.jl +++ b/src/ParametricUtils.jl @@ -126,6 +126,9 @@ function getMeasurementParametric(s::AbstractFactor) return getMeasurementParametric(Z) end +getMeasurementParametric(fct::DFGFactor) = getMeasurementParametric(getFactorType(fct)) +getMeasurementParametric(dfg::AbstractDFG, flb::Symbol) = getMeasurementParametric(getFactor(dfg, flb)) + ## ================================================================================================ ## Parametric solve with Mahalanobis distance - CalcFactor ## ================================================================================================ diff --git a/test/runtests.jl b/test/runtests.jl index a819ecf7..17493912 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -12,6 +12,7 @@ end if TEST_GROUP in ["all", "basic_functional_group"] include("testSphereMani.jl") include("testSpecialOrthogonalMani.jl") +include("testBasicManifolds.jl") include("testSpecialEuclidean2Mani.jl") # start as basic as possible and build from there diff --git a/test/testBasicManifolds.jl b/test/testBasicManifolds.jl new file mode 100644 index 00000000..bf0ddffc --- /dev/null +++ b/test/testBasicManifolds.jl @@ -0,0 +1,27 @@ + +# test Manifolds + +using Manifolds +using Test + +## + +@testset "Basic Manifolds consistency check" begin +## + +w = [-0.0;-0.78;-0.18] + +M = SpecialEuclidean(3) +Mr = M.manifold[2] +pPq = ArrayPartition(zeros(3), exp(Mr, Identity(Mr), hat(Mr, Identity(Mr), w))) +rPc_ = exp(M, Identity(M), hat(M, Identity(M), [zeros(3);w])) +rPc = ArrayPartition(rPc_.parts[1], rPc_.parts[2]) + +@test isapprox(pPq.x[1], rPc.x[1]) +@test isapprox(pPq.x[2], rPc.x[2]) + +## +end + + +## \ No newline at end of file