Merge pull request #734 from JuliaRobotics/24Q1/factors/Pose3Pose3Uni…

…tTrans

New Factor Pose3Pose3UnitTrans

src/factors/Pose3Pose3.jl

@@ -47,11 +47,6 @@ function convert(::Type{PackedPose3Pose3}, obj::Pose3Pose3)
   return PackedPose3Pose3( convert(PackedSamplableBelief, obj.Z) )
 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)]))
@@ -73,3 +68,21 @@ function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa)
     q̂ = Manifolds.compose(M, p, b_m)
     return vee(M, q, log(M, q, q̂)) # coordinates
 end
+
+## Pose3Pose3UnitTrans Factor 
+"""
+  $(TYPEDEF)
+Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation.
+"""
+Base.@kwdef struct Pose3Pose3UnitTrans{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
+  Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)]))
+end
+
+getManifold(::InstanceType{Pose3Pose3UnitTrans}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3)
+
+function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where T
+    M = getManifold(Pose3)
+    q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X))
+    Xc::SVector{6,T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis())
+    return SVector{6,T}(normalize(Xc[1:3])..., Xc[4:6]...)
+end