From caf5cabac7b8512bb6fa2e15fe398e30e5615d9e Mon Sep 17 00:00:00 2001
From: Johan Terblanche <6612981+Affie@users.noreply.github.com>
Date: Sat, 6 Feb 2021 16:12:53 +0200
Subject: [PATCH 1/2] Parametric Point2Point2Range

---
 src/factors/Range2D.jl |  9 ++++++---
 test/testParametric.jl | 27 +++++++++++++++++++++++++++
 2 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl
index e567da11..55a232b7 100644
--- a/src/factors/Range2D.jl
+++ b/src/factors/Range2D.jl
@@ -17,11 +17,14 @@ function (cfo::CalcFactor{<:Point2Point2Range})(rho, theta, xi, lm)
   #
   XX = lm[1] - (rho[1]*cos(theta[1]) + xi[1])
   YY = lm[2] - (rho[1]*sin(theta[1]) + xi[2])
-  #TODO JT - Should this have a sqrt for parametric?
-  # return XX^2 + YY^2 
-  return sqrt(XX^2 + YY^2)
+  return XX^2 + YY^2
 end
 
+#Parametric only function fro Point2Point2Range 
+function (cfo::CalcFactor{<:Point2Point2Range})(rho, xi, lm)
+  # Basically `EuclidDistance`
+  return [rho[1] - norm(lm .- xi)]
+end
 
 passTypeThrough(d::FunctionNodeData{Point2Point2Range}) = d
 
diff --git a/test/testParametric.jl b/test/testParametric.jl
index f4a6e170..4978d7b1 100644
--- a/test/testParametric.jl
+++ b/test/testParametric.jl
@@ -190,3 +190,30 @@ vardict, result, varIds, Σ = IIF.solveFactorGraphParametric!(fg)
 @test isapprox(vardict[:x1].val, [10, 0, 0, 10, 0], atol = 1e-3)
 
 end
+
+
+@testset "Test Parametric PriorPoint2 and Point2Point2Range" begin
+fg = LightDFG( solverParams=SolverParams(algorithms=[:default, :parametric]))
+
+addVariable!(fg, :x1, Point2)
+addVariable!(fg, :l1, Point2)
+addVariable!(fg, :l2, Point2)
+addVariable!(fg, :l3, Point2)
+
+addFactor!(fg, [:l1], PriorPoint2(MvNormal([0., 0], [0.01, 0.01])))
+addFactor!(fg, [:l2], PriorPoint2(MvNormal([1., 0], [0.01, 0.01])))
+addFactor!(fg, [:l3], PriorPoint2(MvNormal([0., 1], [0.01, 0.01])))
+
+addFactor!(fg, [:x1; :l1], Point2Point2Range(Normal(sqrt(2), 0.1)))
+addFactor!(fg, [:x1; :l2], Point2Point2Range(Normal(1.0, 0.1)))
+addFactor!(fg, [:x1; :l3], Point2Point2Range(Normal(1.0, 0.1)))
+
+ensureAllInitialized!(fg)
+
+IIF.initParametricFrom!(fg)
+
+vardict, result, varIds, Σ = IIF.solveFactorGraphParametric(fg) #autodiff=:finite)
+
+@test isapprox(vardict[:x1].val, [1, 1], atol = 1e-3)
+
+end
\ No newline at end of file

From 53ad174736d5141ad6e9a536a364bc7709573161 Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <6612981+Affie@users.noreply.github.com>
Date: Sat, 6 Feb 2021 18:22:46 +0200
Subject: [PATCH 2/2] Update Range2D.jl

---
 src/factors/Range2D.jl | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/factors/Range2D.jl b/src/factors/Range2D.jl
index 55a232b7..72db4baf 100644
--- a/src/factors/Range2D.jl
+++ b/src/factors/Range2D.jl
@@ -17,7 +17,7 @@ function (cfo::CalcFactor{<:Point2Point2Range})(rho, theta, xi, lm)
   #
   XX = lm[1] - (rho[1]*cos(theta[1]) + xi[1])
   YY = lm[2] - (rho[1]*sin(theta[1]) + xi[2])
-  return XX^2 + YY^2
+  return sqrt(XX^2 + YY^2)
 end
 
 #Parametric only function fro Point2Point2Range