diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index 31670440..a00b1c10 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -59,6 +59,26 @@ function compare(a::PriorPose3ZRP, b::PriorPose3ZRP; tol::Float64=1e-10) end +""" + $(TYPEDEF) + +Prior for vector measurements on Pose3. +""" +Base.@kwdef struct PriorPose3Ref{T<:SamplableBelief} <: IncrementalInference.AbstractPrior + Z::T + ref::SVector{3, Float64} + partial::Tuple{Int,Int,Int} = (4,5,6) +end + +getManifold(::PriorPose3Ref) = TranslationGroup(3) + +function (cf::CalcFactor{<:PriorPose3Ref})(p_m, p) + n_r = cf.factor.ref + nRp = p.x[2] + n_m = nRp * p_m + return n_r - n_m +end + ##============================================================================== ## Partial Pose3 Pose3 Factors