Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

update tests for IIF v0.22 refactoring #408

Merged
merged 1 commit into from
Feb 11, 2021
Merged

update tests for IIF v0.22 refactoring #408

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 8 additions & 7 deletions test/testPoint2Point2.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ using Test

@testset "basic Point2Point2 test" begin

global fg = initfg()
fg = initfg()

addVariable!(fg, :x0, Point2)
addFactor!(fg, [:x0], PriorPoint2(MvNormal(zeros(2), Matrix{Float64}(LinearAlgebra.I, 2,2))))
Expand All @@ -17,14 +17,15 @@ addVariable!(fg, :x1, Point2)
addFactor!(fg, [:x0;:x1], Point2Point2(MvNormal([10;0.0], Matrix{Float64}(LinearAlgebra.I, 2,2))))

tree, smt, hist = solveTree!(fg)
# global tree = wipeBuildNewTree!(fg)
# tree = wipeBuildNewTree!(fg)
# inferOverTree!(fg, tree)

@test sum( abs.(Statistics.mean(getVal(fg, :x0),dims=2) .- [0.0;0]) .< [1.0;1.0]) == 2
@test sum( abs.(Statistics.mean(getVal(fg, :x1),dims=2) .- [10.0;0]) .< [1.0;1.0]) == 2

end

##

# drawGraph(fg, show=true)
#
Expand All @@ -44,8 +45,8 @@ end

##

global N=100 # return to 200
global fg = initfg()
N=100 # return to 200
fg = initfg()

addVariable!(fg, :x0, Point2, N=N)
addFactor!(fg, [:x0], PriorPoint2(MvNormal([100.0;0], diagm(ones(2)) )))
Expand All @@ -65,7 +66,7 @@ addFactor!(fg, [:x1;:l1], Point2Point2Range(Normal(100.0, 1.0)) )
##

tree, smt, hist = solveTree!(fg)
# global tree = wipeBuildNewTree!(fg)
# tree = wipeBuildNewTree!(fg)
# inferOverTree!(fg, tree, N=N)


Expand All @@ -75,10 +76,10 @@ tree, smt, hist = solveTree!(fg)
@test 0.1*N < sum( -10 .< getVal(fg, :l1)[1,:] .< 10 )
@test 0.1*N < sum( 90 .< getVal(fg, :l1)[2,:] .< 110 )

global voidsel1 = 10.0 .< getVal(fg, :l1)[1,:]
voidsel1 = 10.0 .< getVal(fg, :l1)[1,:]
@test sum( getVal(fg, :l1)[2,voidsel1] .< 70 ) < 0.35*N

global voidsel2 = 10.0 .< getVal(fg, :l1)[2,:]
voidsel2 = 10.0 .< getVal(fg, :l1)[2,:]
@test sum( getVal(fg, :l1)[1,voidsel2] .< 70 ) < 0.35*N

@test sum( 120 .< abs.(getVal(fg, :l1)[1,:]) ) < 0.35*N
Expand Down
181 changes: 101 additions & 80 deletions test/testpackingconverters.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,23 +11,26 @@ import DistributedFactorGraphs: packVariableNodeData, unpackVariableNodeData
# import RoME: compare


##

@testset "test PriorPoint2" begin

global prpt2 = PriorPoint2( MvNormal([0.25;0.75], Matrix(Diagonal([1.0;2.0]))) )
global prpt2 = PriorPoint2( MvNormal([0.25;0.75], Matrix(Diagonal([1.0;2.0]))) )

global pprpt2 = convert(PackedPriorPoint2, prpt2)
global uprpt2 = convert(PriorPoint2, pprpt2)
global pprpt2 = convert(PackedPriorPoint2, prpt2)
global uprpt2 = convert(PriorPoint2, pprpt2)

@test norm(prpt2.Z.μ - uprpt2.Z.μ) < 1e-8
@test norm(prpt2.Z.μ - uprpt2.Z.μ) < 1e-8

@test norm(prpt2.Z.Σ.mat - uprpt2.Z.Σ.mat) < 1e-8
@test norm(prpt2.Z.Σ.mat - uprpt2.Z.Σ.mat) < 1e-8

# test backwards compatibility, TODO remove
global prpt2 = PriorPoint2( MvNormal([0.25;0.75], Matrix(Diagonal([1.0;2.0].^2)) ) )
# test backwards compatibility, TODO remove
global prpt2 = PriorPoint2( MvNormal([0.25;0.75], Matrix(Diagonal([1.0;2.0].^2)) ) )

end


##


N = 100
Expand Down Expand Up @@ -77,46 +80,52 @@ upv1data = unpackVariableNodeData(fg, packedv1data)

end

##


@testset "test conversions of Pose2Pose2" begin

global dd = convert(PackedPose2Pose2, ppc)
global upd = convert(RoME.Pose2Pose2, dd)
global dd = convert(PackedPose2Pose2, ppc)
global upd = convert(RoME.Pose2Pose2, dd)

# TODO -- fix ambiguity in compare function
@test RoME.compare(ppc, upd)
# TODO -- fix ambiguity in compare function
@test RoME.compare(ppc, upd)

global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose2Pose2}, DFG.getSolverData(f2))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose2Pose2}}, packeddata)
global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose2Pose2}, DFG.getSolverData(f2))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose2Pose2}}, packeddata)

# TODO -- fix ambibuity in compare function
@test DFG.compare(DFG.getSolverData(f2), unpackeddata)

# TODO -- fix ambibuity in compare function
@test DFG.compare(DFG.getSolverData(f2), unpackeddata)
end


@testset "test conversions of Pose2Point2BearingRange" begin
# and a second pose
global v3 = addVariable!(fg, :l1, Point2, N=N)
# ppc = Pose2Point2BearingRange([50.0;0.0;pi/2], 0.01*Matrix{Float64}(LinearAlgebra.I, 2,2), [1.0])
global ppbr = Pose2Point2BearingRange(
Normal(0.0, 0.005 ),
Normal(50, 0.5) )
global f3 = addFactor!(fg, [:x2;:l1], ppbr)

global dd = convert(PackedPose2Point2BearingRange, ppbr)
global upd = convert(
RoME.Pose2Point2BearingRange,
dd
)
# and a second pose
global v3 = addVariable!(fg, :l1, Point2, N=N)
# ppc = Pose2Point2BearingRange([50.0;0.0;pi/2], 0.01*Matrix{Float64}(LinearAlgebra.I, 2,2), [1.0])
global ppbr = Pose2Point2BearingRange(
Normal(0.0, 0.005 ),
Normal(50, 0.5) )
global f3 = addFactor!(fg, [:x2;:l1], ppbr)

global dd = convert(PackedPose2Point2BearingRange, ppbr)
global upd = convert(
RoME.Pose2Point2BearingRange,
dd
)


global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose2Point2BearingRange}, DFG.getSolverData(f3))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose2Point2BearingRange}}, packeddata) # IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose2Point2BearingRange{Normal{Float64},Normal{Float64}}}}

global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose2Point2BearingRange}, DFG.getSolverData(f3))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose2Point2BearingRange}}, packeddata) # IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose2Point2BearingRange{Normal{Float64},Normal{Float64}}}}
@test ppbr.bearing.μ == unpackeddata.fnc.usrfnc!.bearing.μ
@test ppbr.bearing.σ == unpackeddata.fnc.usrfnc!.bearing.σ

@test ppbr.bearing.μ == unpackeddata.fnc.usrfnc!.bearing.μ
@test ppbr.bearing.σ == unpackeddata.fnc.usrfnc!.bearing.σ
@test ppbr.range.μ == unpackeddata.fnc.usrfnc!.range.μ
@test ppbr.range.σ == unpackeddata.fnc.usrfnc!.range.σ

@test ppbr.range.μ == unpackeddata.fnc.usrfnc!.range.μ
@test ppbr.range.σ == unpackeddata.fnc.usrfnc!.range.σ
end

# parameters
Expand All @@ -132,61 +141,72 @@ global v1 = addVariable!(fg, :x1, Pose3, N=N) # 0.1*randn(6,N)
global ipp = PriorPose3(MvNormal(zeros(6), initCov) )
global f1 = addFactor!(fg,[v1], ipp)

##

@testset "test conversions of PriorPose3" begin

global dd = convert(PackedPriorPose3, ipp)
global upd = convert(RoME.PriorPose3, dd)
##

# @test TransformUtils.compare(ipp.Zi, upd.Zi)
@test norm(ipp.Zi.μ - upd.Zi.μ) < 1e-10
@test norm(ipp.Zi.Σ.mat - upd.Zi.Σ.mat) < 1e-8
global dd = convert(PackedPriorPose3, ipp)
global upd = convert(RoME.PriorPose3, dd)

global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPriorPose3}, DFG.getSolverData(f1))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.PriorPose3}}, packeddata)
# @test TransformUtils.compare(ipp.Zi, upd.Zi)
@test norm(ipp.Zi.μ - upd.Zi.μ) < 1e-10
@test norm(ipp.Zi.Σ.mat - upd.Zi.Σ.mat) < 1e-8

# TODO -- fix ambibuity in compare function
@test compareAll(DFG.getSolverData(f1), unpackeddata, skip=[:fnc;])
@test compareAll(DFG.getSolverData(f1).fnc, unpackeddata.fnc, skip=[:params;:threadmodel;:cpt;:usrfnc!])
@test compareAll(DFG.getSolverData(f1).fnc.usrfnc!, unpackeddata.fnc.usrfnc!, skip=[:Zi;])
@test compareAll(DFG.getSolverData(f1).fnc.usrfnc!.Zi, unpackeddata.fnc.usrfnc!.Zi, skip=[:Σ;])
@test compareAll(DFG.getSolverData(f1).fnc.usrfnc!.Zi.Σ, unpackeddata.fnc.usrfnc!.Zi.Σ)
@test compareAll(DFG.getSolverData(f1).fnc.cpt[1], unpackeddata.fnc.cpt[1], skip=[:factormetadata;:activehypo])
# @test compareAll(DFG.getSolverData(f1).fnc.params, unpackeddata.fnc.params)
@warn "threadmodel is not defined, fix with DFG"
# @test compareAll(DFG.getSolverData(f1).fnc.threadmodel, unpackeddata.fnc.threadmodel)
global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPriorPose3}, DFG.getSolverData(f1))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.PriorPose3}}, packeddata)

# TODO: Ref above
packedv1data = packVariableNodeData(fg, DFG.getSolverData(v1))
upv1data = unpackVariableNodeData(fg, packedv1data)
# global packedv1data = convert(IncrementalInference.PackedVariableNodeData, DFG.getSolverData(v1))
# global upv1data = convert(IncrementalInference.VariableNodeData, packedv1data)
# TODO -- fix ambibuity in compare function
@test compareAll(DFG.getSolverData(f1), unpackeddata, skip=[:fnc;])
@test compareAll(DFG.getSolverData(f1).fnc, unpackeddata.fnc, skip=[:params;:threadmodel;:cpt;:usrfnc!])
@test compareAll(DFG.getSolverData(f1).fnc.usrfnc!, unpackeddata.fnc.usrfnc!, skip=[:Zi;])
@test compareAll(DFG.getSolverData(f1).fnc.usrfnc!.Zi, unpackeddata.fnc.usrfnc!.Zi, skip=[:Σ;])
@test compareAll(DFG.getSolverData(f1).fnc.usrfnc!.Zi.Σ, unpackeddata.fnc.usrfnc!.Zi.Σ)

@test compareAll(DFG.getSolverData(v1), upv1data, skip=[:softtype;])
@test compareAll(DFG.getSolverData(v1).softtype, upv1data.softtype)
@error "not comparing CCW.fnc.cpt, see this test file for details -- pending IIF #825, and DFG #590"
# see https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/issues/590#issuecomment-776838053
# @test compareAll(DFG.getSolverData(f1).fnc.cpt[1], unpackeddata.fnc.cpt[1], skip=[:factormetadata;:activehypo])

end
# @test compareAll(DFG.getSolverData(f1).fnc.params, unpackeddata.fnc.params)
@warn "threadmodel is not defined, fix with DFG"
# @test compareAll(DFG.getSolverData(f1).fnc.threadmodel, unpackeddata.fnc.threadmodel)

# TODO: Ref above
packedv1data = packVariableNodeData(fg, DFG.getSolverData(v1))
upv1data = unpackVariableNodeData(fg, packedv1data)
# global packedv1data = convert(IncrementalInference.PackedVariableNodeData, DFG.getSolverData(v1))
# global upv1data = convert(IncrementalInference.VariableNodeData, packedv1data)

@test compareAll(DFG.getSolverData(v1), upv1data, skip=[:softtype;])
@test compareAll(DFG.getSolverData(v1).softtype, upv1data.softtype)

##

end

##

pp3 = Pose3Pose3( MvNormal([25.0;0;0;0;0;0], odoCov) )
v2 = addVariable!(fg, :x2, Pose3)
f2 = addFactor!(fg, [:x1, :x2], pp3)


@testset "test conversions of Pose3Pose3" begin
global dd = convert(PackedPose3Pose3, pp3)
global upd = convert(RoME.Pose3Pose3, dd)

global dd = convert(PackedPose3Pose3, pp3)
global upd = convert(RoME.Pose3Pose3, dd)


@test norm(pp3.Zij.μ - upd.Zij.μ) < 1e-10
@test norm(pp3.Zij.Σ.mat - upd.Zij.Σ.mat) < 1e-8
@test norm(pp3.Zij.μ - upd.Zij.μ) < 1e-10
@test norm(pp3.Zij.Σ.mat - upd.Zij.Σ.mat) < 1e-8

global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose3Pose3}, DFG.getSolverData(f2))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose3Pose3}}, packeddata)
global packeddata = convert(IncrementalInference.PackedFunctionNodeData{RoME.PackedPose3Pose3}, DFG.getSolverData(f2))
global unpackeddata = convert(IncrementalInference.FunctionNodeData{IIF.CommonConvWrapper{RoME.Pose3Pose3}}, packeddata)

# TODO -- fix ambibuity in compare function
@test DFG.compare(DFG.getSolverData(f2), unpackeddata)

# TODO -- fix ambibuity in compare function
@test DFG.compare(DFG.getSolverData(f2), unpackeddata)
end


Expand All @@ -213,34 +233,35 @@ global f3 = addFactor!(fg,[:x1;:x2],odoc, nullhypo=0.5)
# @test DFG.compare(DFG.getSolverData(f3), unpackeddata)
# end


##

@testset "test conversions of PartialPriorRollPitchZ" begin

global prpz = PartialPriorRollPitchZ(MvNormal([0.0;0.5],0.1*diagm([1.0;1])),Normal(3.0,0.5))
global prpz = PartialPriorRollPitchZ(MvNormal([0.0;0.5],0.1*diagm([1.0;1])),Normal(3.0,0.5))

global pprpz = convert(PackedPartialPriorRollPitchZ, prpz)
global unp = convert(PartialPriorRollPitchZ, pprpz)
global pprpz = convert(PackedPartialPriorRollPitchZ, prpz)
global unp = convert(PartialPriorRollPitchZ, pprpz)

@test RoME.compare(prpz, unp)
@test RoME.compare(prpz, unp)

end


@testset "test conversions of PartialPose3XYYaw" begin
global xyy = PartialPose3XYYaw(
MvNormal( [1.0;2.0],
0.1*diagm([1.0;1]) ),
Normal(0.5, 0.1)
)

global pxyy = convert(PackedPartialPose3XYYaw, xyy)
global unp = convert(PartialPose3XYYaw, pxyy)
global xyy = PartialPose3XYYaw(
MvNormal( [1.0;2.0],
0.1*diagm([1.0;1]) ),
Normal(0.5, 0.1)
)

@test RoME.compare(xyy, unp)
end
global pxyy = convert(PackedPartialPose3XYYaw, xyy)
global unp = convert(PartialPose3XYYaw, pxyy)

@test RoME.compare(xyy, unp)
end

##

# @testset "test conversions of PartialPose3XYYawNH" begin
#
Expand Down