Merge pull request #87 from JuliaRobotics/hotfix/point2point2constructor

Hotfix/point2point2constructor

src/Point2D.jl

@@ -50,19 +50,20 @@ end
 
 mutable struct Point2Point2{D <: SamplableBelief} <: FunctorPairwise #BetweenPoses
     Zij::D
-    Point2Point2() = new()
-    Point2Point2(x) = new(x)
+    Point2Point2{T}() where T = new{T}()
+    Point2Point2{T}(x::T) where {T <: Sampleable} = new{T}(x)
 end
+Point2Point2(x::T) where {T <: Sampleable} = Point2Point2{T}(x)
 function getSample(pp2::Point2Point2, N::Int=1)
   return (rand(pp2.Zij,N),  )
 end
-function (pp2r::Point2Point2)(
+function (pp2r::Point2Point2{T})(
             res::Array{Float64},
             userdata::FactorMetadata,
             idx::Int,
             meas::Tuple,
             xi::Array{Float64,2},
-            xj::Array{Float64,2} )
+            xj::Array{Float64,2} ) where T
   #
   res[1]  = meas[1][1,idx] - (xj[1,idx] - xi[1,idx])
   res[2]  = meas[1][2,idx] - (xj[2,idx] - xi[2,idx])

test/runtests.jl

@@ -23,6 +23,8 @@ println("[TEST] Linear array function evaluations...")
 include("testDidsonFunctions.jl")
 println("[SUCCESS]")
 
+include("testPoint2Point2.jl")
+
 println("[TEST] Pose2 evaluations...")
 include("TestPoseAndPoint2Constraints.jl")
 println("[SUCCESS]")

test/testPoint2Point2.jl

@@ -0,0 +1,21 @@
+using RoME, Distributions
+
+using Base: Test
+
+@testset "basic Point2Point2 test" begin
+
+fg = initfg()
+
+addNode!(fg, :x0, Point2)
+addFactor!(fg, [:x0], PriorPoint2(MvNormal(zeros(2), eye(2))))
+
+addNode!(fg, :x1, Point2)
+addFactor!(fg, [:x0;:x1], Point2Point2(MvNormal([10;0.0], eye(2))))
+
+tree = wipeBuildNewTree!(fg)
+inferOverTree!(fg, tree)
+
+@test sum( abs.(Base.mean(getVal(fg, :x0),2) - [0.0;0]) .< [0.5;0.5]) == 2
+@test sum( abs.(Base.mean(getVal(fg, :x1),2) - [10.0;0]) .< [0.5;0.5]) == 2
+
+end