import Rotations as _Rotations fix,

See JuliaRobotics/RoME.jl#522 Co-authored-by: Johannes Terblanche <Affie@users.noreply.github.com>
JuliaRobotics · Oct 8, 2021 · 23d51bb · 23d51bb
1 parent 1f73712
commit 23d51bb
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 9 deletions.
diff --git a/src/Caesar.jl b/src/Caesar.jl
@@ -17,6 +17,8 @@ using Manifolds
 @reexport using KernelDensityEstimate
 @reexport using Distributions
 
+import Rotations as _Rotations
+
 using
   Pkg,
   DelimitedFiles,
@@ -26,7 +28,6 @@ using
   IncrementalInference,
   TransformUtils,
   CoordinateTransformations,
-  Rotations,
   JSON,
   JSON2,
   FileIO,

diff --git a/src/images/Pose2AprilTag4Corners.jl b/src/images/Pose2AprilTag4Corners.jl
@@ -66,7 +66,7 @@ Related
 
 `AprilTags.detect`, `PackedPose2AprilTag4Corners`, [`generateCostAprilTagsPreimageCalib`](@ref)
 """
-struct Pose2AprilTag4Corners{T <: SamplableBelief, F <: Function} <: AbstractRelativeRoots
+struct Pose2AprilTag4Corners{T <: SamplableBelief, F <: Function} <: IIF.AbstractManifoldMinimize
   # 4 corners as detected by AprilTags
   corners::NTuple{4,Tuple{Float64,Float64}}
   # homography matrix
@@ -135,7 +135,7 @@ function _AprilTagToPose2(corners,
   pose, err1 = AprilTags.tagOrthogonalIteration(corners, homography, f_width, f_height, c_width, c_height, taglength=taglength_)
   cVt = Translation((pose[1:3,4])...)
   # bRc = bRy * yRc 
-  bRc = Rotations.Quat(1/sqrt(2),0,1/sqrt(2),0) * Rotations.Quat(1/sqrt(2),0,0,-1/sqrt(2))
+  bRc = _Rotations.Quat(1/sqrt(2),0,1/sqrt(2),0) * _Rotations.Quat(1/sqrt(2),0,0,-1/sqrt(2))
   # for tag in body frame == bTt
   bTt = LinearMap(bRc) ∘ cVt
 
@@ -224,7 +224,7 @@ end
 
 function getSample( pat4c::CalcFactor{<:Pose2AprilTag4Corners} )
   #
-  M = getManifold(pat4c.factor.Zij.z)
+  M = getManifold(pat4c.factor.Zij)
   ϵ = getPointIdentity(Pose2)
 
   X = sampleTangent(M, pat4c.factor.Zij.z, ϵ)
@@ -237,7 +237,7 @@ function (pat4c::CalcFactor{<:Pose2AprilTag4Corners})(X,
   #
 
   @assert X isa ProductRepr "Pose2AprilTag4Corners expects measurement sample X to be a Manifolds tangent vector, not coordinate or point representation.  Got X=$X"
-  M = getManifold(pat4c.factor.Zij.z)
+  M = getManifold(pat4c.factor.Zij)
   q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) #for groups
   #TODO allocalte for vee! see Manifolds #412, fix for AD
   Xc = zeros(3)

diff --git a/test/testPose2AprilTag4Corner.jl b/test/testPose2AprilTag4Corner.jl
@@ -4,6 +4,7 @@ using Caesar
 using AprilTags
 using Optim
 using Statistics
+using Manifolds
 
 using FileIO
 
@@ -79,8 +80,9 @@ meas = sampleFactor(IIF._getCCW(atf),2)
 
 pts = approxConv(fg, DFG.ls(fg,:tag17)[1], :tag17)
 
-@test size(pts,1) == 3
-@test 1 < size(pts,2)
+@test 1 < length(pts)
+@test length(pts[1].parts[1]) == 2
+@test size(pts[1].parts[2]) == (2,2)
 
 
 ## test packing of factor
@@ -131,9 +133,13 @@ uf4_ = getFactorType(fg_, DFG.ls(fg_, :tag17)[1])
 
 ## Test deconvolution
 
-meas = approxDeconv(fg, DFG.ls(fg, :tag17)[1])
+pred, meas = approxDeconv(fg, DFG.ls(fg, :tag17)[1])
 
-@test sum(Statistics.mean(meas[1] - meas[2], dims=2) .< [0.1, 0.1, 0.1]) == 3
+M = Manifolds.SpecialEuclidean(2)
+pred_ = exp.(Ref(M), Ref(identity_element(M)), pred)
+meas_ = exp.(Ref(M), Ref(identity_element(M)), meas)
+
+@test isapprox( M, mean(M,pred_),  mean(M, meas_), atol=0.5)
 
 
 ## test preimage search ("SLAM-wise calibration")