Merge pull request #1626 from JuliaRobotics/22Q3/enh/manitestimpr

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
 ## ================================================================================================

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

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
+
+
+##