Merge pull request #1301 from JuliaRobotics/21Q2/bugfix/productRepr

bugfix for ProductRepr and include SE(2) test
JuliaRobotics · Jul 8, 2021 · 589e5e1 · 589e5e1
2 parents 633d5f2 + 1a4ba9b
commit 589e5e1
Show file tree

Hide file tree

Showing 4 changed files with 25 additions and 10 deletions.
diff --git a/src/ApproxConv.jl b/src/ApproxConv.jl
@@ -871,7 +871,8 @@ function findRelatedFromPotential(dfg::AbstractDFG,
   # vdim = getVariableDim(DFG.getVariable(dfg, target))
 
   # TODO -- better to upsample before the projection
-  Ndim = length(pts[1])
+  # Ndim = length(pts[1]) # not used, should maybe be: 
+  # Ndim = manifold_dimension(M)
   Npoints = length(pts) # size(pts,2)
   # Assume we only have large particle population sizes, thanks to addNode!
   M = getManifold(getVariableType(dfg, target))

diff --git a/src/NumericalCalculations.jl b/src/NumericalCalculations.jl
@@ -301,10 +301,11 @@ function _solveCCWNumeric!( ccwl::Union{CommonConvWrapper{F},
   retval = _solveLambdaNumeric(getFactorType(ccwl), _hypoObj, cpt_.res, cpt_.X[smpid], ccwl.vartypes[sfidx](), islen1)
 
   # Check for NaNs
-  if sum(isnan.(retval)) != 0
-    @error "$(ccwl.usrfnc!), ccw.thrid_=$(thrid), got NaN, smpid = $(smpid), r=$(retval)\n"
-    return nothing
-  end
+  # FIXME isnan for manifold ProductRepr
+  # if sum(isnan.(retval)) != 0
+    # @error "$(ccwl.usrfnc!), ccw.thrid_=$(thrid), got NaN, smpid = $(smpid), r=$(retval)\n"
+    # return nothing
+  # end
 
   # insert result back at the correct variable element location
   cpt_.X[smpid] .= retval

diff --git a/test/runtests.jl b/test/runtests.jl
@@ -5,6 +5,7 @@ using Test
 
 include("testSphereMani.jl")
 include("testSpecialOrthogonalMani.jl")
+include("testSpecialEuclidean2Mani.jl")
 
 include("testCliqSolveDbgUtils.jl")
 include("TestModuleFunctions.jl")

diff --git a/test/testSpecialEuclidean2Mani.jl b/test/testSpecialEuclidean2Mani.jl
@@ -19,7 +19,7 @@ pT = getPointType(SpecialEuclidean2)
 # @test pT == ProductRepr{Tuple{MVector{2, Float64}, MMatrix{2, 2, Float64, 4}}}
 pϵ = getPointIdentity(SpecialEuclidean2)
 # @test_broken pϵ == ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0]))
-@test_broken pϵ == ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])
+@test all(isapprox.(pϵ,ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0])).parts)
 
 @test is_point(getManifold(SpecialEuclidean2), getPointIdentity(SpecialEuclidean2))
 
@@ -32,7 +32,7 @@ v0 = addVariable!(fg, :x0, SpecialEuclidean2)
 mp = ManifoldPrior(SpecialEuclidean(2), ProductRepr(@MVector([0.0,0.0]), @MMatrix([1.0 0.0; 0.0 1.0])), MvNormal([0.01, 0.01, 0.01]))
 p = addFactor!(fg, [:x0], mp)
 
-@test_broken doautoinit!(fg, :x0)
+doautoinit!(fg, :x0)
 
 ##
 vnd = getVariableSolverData(fg, :x0)
@@ -41,14 +41,26 @@ vnd = getVariableSolverData(fg, :x0)
 
 ##
 v1 = addVariable!(fg, :x1, SpecialEuclidean2)
-mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([pi], [0.01]))
+mf = ManifoldFactor(SpecialEuclidean(2), MvNormal([1,2,pi/4], [0.01,0.01,0.01]))
 f = addFactor!(fg, [:x0, :x1], mf)
 
-@test_broken doautoinit!(fg, :x1)
+doautoinit!(fg, :x1)
+
+vnd = getVariableSolverData(fg, :x1)
+@test all(isapprox.(mean(vnd.val), ProductRepr([1.0,2.0], [0.7071 -0.7071; 0.7071 0.7071]), atol=0.1).parts)
+@test all(is_point.(Ref(M), vnd.val))
 
 ##
 smtasks = Task[]
-@test_broken solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
+solveTree!(fg; smtasks, verbose=true, recordcliqs=ls(fg))
 # hists = fetchCliqHistoryAll!(smtasks);
 
+vnd = getVariableSolverData(fg, :x0)
+@test all(isapprox.(mean(vnd.val), ProductRepr([0.0,0.0], [1.0 0.0; 0.0 1.0]), atol=0.1).parts)
+@test all(is_point.(Ref(M), vnd.val))
+
+vnd = getVariableSolverData(fg, :x1)
+@test all(isapprox.(mean(vnd.val), ProductRepr([1.0,2.0], [0.7071 -0.7071; 0.7071 0.7071]), atol=0.1).parts)
+@test all(is_point.(Ref(M), vnd.val))
+
 end