From 58e34a2c61cfae7110c1cc816fc738c491a015a5 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 30 Aug 2022 16:27:53 +0200 Subject: [PATCH 1/2] Untested PriorPose3Ref --- src/factors/PartialPose3.jl | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index 31670440..21769ae0 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -59,6 +59,25 @@ 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} +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 From 3b99f9a2e3b56f7ab10b933a46200e3c656ad737 Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 30 Aug 2022 16:43:47 +0200 Subject: [PATCH 2/2] Add partial field to PriorPose3Ref --- src/factors/PartialPose3.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/factors/PartialPose3.jl b/src/factors/PartialPose3.jl index 21769ae0..a00b1c10 100644 --- a/src/factors/PartialPose3.jl +++ b/src/factors/PartialPose3.jl @@ -67,6 +67,7 @@ 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)