From 4c6b7395c26e195817a56a7a0db41cb0b999050e Mon Sep 17 00:00:00 2001
From: dehann <dehann.fourie@gmail.com>
Date: Wed, 8 Aug 2018 09:46:02 -0400
Subject: [PATCH] improve testing and bugfix

---
 src/DynPose2D.jl      | 28 ++++++++++++++++++++++++++--
 test/testDynPose2D.jl | 32 ++++++++++++++++++++++++++++++++
 2 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/src/DynPose2D.jl b/src/DynPose2D.jl
index f9fb7a67..73404348 100644
--- a/src/DynPose2D.jl
+++ b/src/DynPose2D.jl
@@ -88,8 +88,28 @@ end
 
 
 
+## Compare functions
+
+function compare(a::MvNormal, b::MvNormal; tol::Float64=1e-10)::Bool
+  TP = true
+  TP = TP && norm(a.μ - b.μ)<tol
+  TP = TP && sum(norm.(a.Σ.mat - b.Σ.mat))<tol
+  return TP
+end
+
+
+function compare(a::DynPose2VelocityPrior, b::DynPose2VelocityPrior)::Bool
+  RoME.compare(a.Zpose, b.Zpose) && RoME.compare(a.Zvel, b.Zvel)
+end
 
 
+function compare(a::VelPose2VelPose2, b::VelPose2VelPose2; tol::Float64=1e-10)::Bool
+  TP = true
+  TP = TP && RoME.compare(a.Zpose, b.Zpose)
+  TP = TP && RoME.compare(a.Zvel, b.Zvel)
+  TP = TP && norm(a.reuseres - b.reuseres) < tol
+  return TP
+end
 
 
 ## Packing types
@@ -121,7 +141,7 @@ mutable struct PackedVelPose2VelPose2 <: IncrementalInference.PackedInferenceTyp
 end
 
 function convert(::Type{PackedVelPose2VelPose2}, d::VelPose2VelPose2)
-  return PackedVelPose2VelPose2(string(d.Zpose),string(d.Zvel))
+  return PackedVelPose2VelPose2(string(d.Zpose.z),string(d.Zvel))
 end
 function convert(::Type{VelPose2VelPose2}, d::PackedVelPose2VelPose2)
   posedistr = extractdistribution(d.strpose)
@@ -139,7 +159,7 @@ mutable struct PackedDynPose2Pose2 <: IncrementalInference.PackedInferenceType
 end
 
 function convert(::Type{PackedDynPose2Pose2}, d::DynPose2Pose2)
-  return PackedDynPose2Pose2(string(d.Zpose))
+  return PackedDynPose2Pose2(string(d.Zpose.z))
 end
 function convert(::Type{DynPose2Pose2}, d::PackedDynPose2Pose2)
   posedistr = extractdistribution(d.strpose)
@@ -147,4 +167,8 @@ function convert(::Type{DynPose2Pose2}, d::PackedDynPose2Pose2)
 end
 
 
+
+
+
+
 #
diff --git a/test/testDynPose2D.jl b/test/testDynPose2D.jl
index bc350276..7184ad79 100644
--- a/test/testDynPose2D.jl
+++ b/test/testDynPose2D.jl
@@ -50,6 +50,38 @@ warn("wrapRad issue, accepting 80% as good enough until issue JuliaRobotics/RoME
 end
 
 
+@testset "test distribution compare functions..." begin
+
+mu = randn(6)
+mv1 = MvNormal(deepcopy(mu), eye(6))
+mv2 = MvNormal(deepcopy(mu), eye(6))
+mv3 = MvNormal(randn(6), eye(6))
+@test RoME.compare(mv1, mv2)
+@test !RoME.compare(mv1, mv3)
+@test !RoME.compare(mv2, mv3)
+
+end
+
+
+@testset "test DynPose2 packing converters..." begin
+
+pp0 = DynPose2VelocityPrior(MvNormal(zeros(3), diagm([0.01; 0.01; 0.001].^2)),
+                            MvNormal([10.0;0], diagm([0.1; 0.1].^2)))
+
+pp = convert(PackedDynPose2VelocityPrior, pp0)
+ppu = convert(DynPose2VelocityPrior, pp)
+
+@test RoME.compare(pp0, ppu)
+
+dp2dp2 = VelPose2VelPose2(MvNormal([10.0;0;0], diagm([0.01;0.01;0.001].^2)),
+                          MvNormal([0.0;0], diagm([0.1; 0.1].^2)))
+
+pp = convert(PackedVelPose2VelPose2, dp2dp2)
+ppu = convert(VelPose2VelPose2, pp)
+
+@test RoME.compare(dp2dp2, ppu)
+
+end