From 35feb8d51f5f6c8dc6301e8d59ef731bf96d62a1 Mon Sep 17 00:00:00 2001
From: Johan <terblanche.j@gmail.com>
Date: Fri, 29 Jan 2021 10:00:38 +0200
Subject: [PATCH 1/2] Fix and test Pose2Point2

---
 src/factors/Pose2Point2.jl |  2 +-
 test/runtests.jl           |  3 ++-
 test/testPose2Point2.jl    | 25 +++++++++++++++++++++++++
 3 files changed, 28 insertions(+), 2 deletions(-)
 create mode 100644 test/testPose2Point2.jl

diff --git a/src/factors/Pose2Point2.jl b/src/factors/Pose2Point2.jl
index 5bed203a..88ac9aeb 100644
--- a/src/factors/Pose2Point2.jl
+++ b/src/factors/Pose2Point2.jl
@@ -33,7 +33,7 @@ function (cfo::CalcFactor{<:Pose2Point2})(res::AbstractVector{<:Real},
                                           wLj )
   #
 
-  wLj_pred = SE2(wXi)*SE2([meas[1];0.0])
+  wLj_pred = SE2(wXi)*SE2([meas;0.0])
   res[1:2] .= wLj .- se2vee(wLj_pred)[1:2]
 
   res .^= 2
diff --git a/test/runtests.jl b/test/runtests.jl
index b8312e87..b955560f 100644
--- a/test/runtests.jl
+++ b/test/runtests.jl
@@ -31,7 +31,8 @@ testfiles = [
 "testpackingconverters.jl";
 "TestDefaultFGInitialization.jl";
 "testAccumulateFactors.jl";
-"testDeadReckoningTether.jl"; ]
+"testDeadReckoningTether.jl"; 
+"testPoint2Point2.jl"]
 
 ## Tests not ready yet
 # "HexagonalLightGraphs.jl"
diff --git a/test/testPose2Point2.jl b/test/testPose2Point2.jl
new file mode 100644
index 00000000..94056cf6
--- /dev/null
+++ b/test/testPose2Point2.jl
@@ -0,0 +1,25 @@
+using RoME
+using Test
+using Statistics
+
+## MWE Pose2Point2 from #388
+@testset "basic Pose2Point2 test" begin
+
+    fg = initfg()
+
+    addVariable!(fg, :x1, Pose2)
+    addVariable!(fg, :l1, Point2)
+
+    addFactor!(fg, [:x1], PriorPose2(MvNormal([0.,0, 0], [0.01, 0.01, 0.01])))
+
+    addFactor!(fg, [:x1; :l1], Pose2Point2(MvNormal([0.0,-1], [0.1,0.1])))
+
+    ensureAllInitialized!(fg)
+
+    tree, smt, hist = solveTree!(fg)
+
+    @test isapprox(mean(getVal(fg, :x1),dims=2), [0,0,0], atol = 0.05)
+    @test isapprox(mean(getVal(fg, :l1),dims=2), [0,-1], atol = 0.05)
+end
+
+

From bbb041112b0ecc032e935dfde70bd3faedabc05c Mon Sep 17 00:00:00 2001
From: Johannes Terblanche <terblanche.j@gmail.com>
Date: Fri, 29 Jan 2021 10:47:31 +0200
Subject: [PATCH 2/2] Update Project.toml

bump to v0.12.1
---
 Project.toml | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Project.toml b/Project.toml
index d6e92f3b..3964ca10 100644
--- a/Project.toml
+++ b/Project.toml
@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.12.0"
+version = "0.12.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"