Merge pull request #390 from JuliaRobotics/bugfix/21Q1/backport_fixPo…

…se2Point2 Backport: Fix and test Pose2Point2
JuliaRobotics · Jan 29, 2021 · c44a7cb · c44a7cb
2 parents 7c7ee34 + bbb0411
commit c44a7cb
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 3 deletions.
diff --git 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"

diff --git 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
@@ -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
@@ -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
+
+