Merge pull request #689 from JuliaRobotics/23Q3/enh/poserot

New factor Pose3Pose3RotOffset

src/factors/Pose3Pose3.jl

@@ -21,12 +21,17 @@ function (cf::CalcFactor{<:Pose3Pose3})(X, p, q)
     # Xc = zeros(6)
     # vee!(M, Xc, q, log(M, q, q̂))
 
-    # FIXME, should be tangent vector not coordinates -- likely part of ManOpt upgrade
     Xc = vee(M, q, log(M, q, q̂))
     return Xc
 end
 
-
+# function (cf::CalcFactor{<:Pose3Pose3})(X, p, q)  
+#   M = cf.manifold # getManifold(Pose3)
+#   ϵX = exp(M, getPointIdentity(M), X)
+#   q̂ = ArrayPartition(p.x[2]*ϵX.x[1] + p.x[1], p.x[2]*ϵX.x[2])
+#   Xc = vee(M, q, log(M, q, q̂))
+#   return Xc
+# end
 
 #TODO Serialization
 
@@ -49,3 +54,25 @@ end
 
 
 #
+
+##
+Base.@kwdef struct Pose3Pose3RotOffset{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
+  Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)]))
+end
+
+getManifold(::InstanceType{Pose3Pose3RotOffset}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3)
+
+
+# measurement is in frame a, for example imu frame
+# p and q is in frame b, for example body frame
+# bRa is the rotation to get a in the b frame 
+# measurement in frame a is converted to frame b and used to calculate the error
+function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa)
+    M = cf.manifold
+    # measurement in frame a, input is tangent, can also use vector transport 
+    a_m = exp(M, getPointIdentity(M), aX)
+    b_m = ArrayPartition(a_m.x[1], bRa * a_m.x[2]) 
+
+    q̂ = Manifolds.compose(M, p, b_m)
+    return vee(M, q, log(M, q, q̂)) # coordinates
+end

src/variables/VariableTypes.jl

@@ -46,6 +46,10 @@ Future:
 """
 @defVariable Pose3 SpecialEuclidean(3) ArrayPartition(SA[0;0;0.0],SA[1 0 0; 0 1 0; 0 0 1.0])
 
+
+@defVariable Rotation3 SpecialOrthogonal(3) SA[1 0 0; 0 1 0; 0 0 1.0]
+
+
 """
 $(TYPEDEF)
 

test/testPose3.jl

@@ -3,6 +3,7 @@ using RoME
 using Manifolds
 using StaticArrays
 using Test
+using StaticArrays
 
 ##
 @testset "test SE(3) coordinates to homography and back" begin
@@ -67,3 +68,62 @@ np4 = mean(M, getVal(fg, :x4))
 @test isapprox(M, p4, np4, atol=0.1)
 
 end
+
+
+@testset "Test Basic Pose3 with Rotation offset :parametric and :default" begin
+fg = initfg()
+fg.solverParams.graphinit = false
+
+addVariable!(fg, :x0, Pose3)
+prior_distribution = PriorPose3( MvNormal(SA[0., 0, 0, 0, 0, pi/2], diagm(SA[0.1, 0.1, 0.1, 0.01, 0.01, 0.01]).^2))
+prior = addFactor!(fg, [:x0], prior_distribution) 
+
+addVariable!(fg, :bRa, RoME.Rotation3)
+
+odo_distribution = MvNormal(SA[1.0, 0, 0, 0, 0, 0.1], diagm(SA[0.1, 0.1, 0.1, 0.01, 0.01, 0.01].^2))
+
+for i=1:2
+    f = Symbol("x",i-1)
+    t = Symbol("x",i)
+    addVariable!(fg, t, Pose3)
+    addFactor!(fg, [f, t, :bRa], RoME.Pose3Pose3RotOffset(odo_distribution))
+end
+
+prior_distribution = PriorPose3( MvNormal(SA[0, 2.0, 0, 0, 0, pi/2], diagm(SA[0.1, 0.1, 0.1, 0.01, 0.01, 0.01]).^2))
+prior = addFactor!(fg, [:x2], prior_distribution) 
+
+IIF.autoinitParametric!(fg)
+
+r = IIF.solveGraphParametric!(fg)
+
+M = getManifold(Pose3)
+
+p0 = getVal(fg, :x0, solveKey=:parametric)[1]
+p1 = getVal(fg, :x1, solveKey=:parametric)[1]
+p2 = getVal(fg, :x2, solveKey=:parametric)[1]
+R = getVal(fg, :bRa, solveKey=:parametric)[1]
+
+@test isapprox(M, p0, ArrayPartition([0, 0.0, 0], [0 -1 0; 1 0 0; 0 0 1.0]), atol=1e-6)
+@test isapprox(M, p1, ArrayPartition([0, 1.0, 0], [0 -1 0; 1 0 0; 0 0 1.0]), atol=1e-6)
+@test isapprox(M, p2, ArrayPartition([0, 2.0, 0], [0 -1 0; 1 0 0; 0 0 1.0]), atol=1e-6)
+@test isapprox(getPPESuggested(fg, :bRa, :parametric), [0,0,-0.1], atol=1e-6)
+
+# Non-parametric is not working yet because
+# bRa cannot be initialized and solveGraph!(fg) gives an error
+initAll!(fg)
+@test_broken isInitialized(fg, :bRa)
+
+# we initialize it manually and try again
+initVariable!(fg, :bRa, MvNormal([0.0, 0, 0], diagm([0.1, 0.1, 0.1]).^2))
+
+#this time it solves and looks relatively ok
+solveGraph!(fg)
+np0 = mean(getBelief(fg, :x0))
+np1 = mean(getBelief(fg, :x1))
+np2 = mean(getBelief(fg, :x2))
+@test isapprox(M, np0, ArrayPartition([0, 0.0, 0], [0 -1 0; 1 0 0; 0 0 1.0]), atol=1e-1)
+@test isapprox(M, np1, ArrayPartition([0, 1.0, 0], [0 -1 0; 1 0 0; 0 0 1.0]), atol=1e-1)
+@test isapprox(M, np2, ArrayPartition([0, 2.0, 0], [0 -1 0; 1 0 0; 0 0 1.0]), atol=1e-1)
+@test isapprox(getPPESuggested(fg, :bRa), [0,0,-0.1], atol=1e-1)
+
+end