Add Pose3 and mini cleanup

src/NavAbilitySDK.jl

@@ -10,6 +10,7 @@ using JSON
 using UUIDs
 using Downloads
 using HTTP
+using Dates
 
 # for overloading with visualization helpers
 import Base: show
@@ -48,8 +49,9 @@ export Client, Scope
 export QueryDetail, LABEL, SKELETON, SUMMARY, FULL
 export Distribution, Normal, Rayleigh, FullNormal, Uniform, Categorical
 export ManifoldKernelDensity
-export VariableType, Variable
+export Variable
 export FactorData, PriorData, PriorPose2Data, PriorPoint2Data, LinearRelativeData, Pose2Pose2Data, Pose2AprilTag4CornersData, Pose2Point2BearingRangeData, Point2Point2RangeData, MixtureData
+export PriorPose3, Pose3Pose3
 export ScatterAlignPose2Data
 export FactorType, Factor
 

src/navability/entities/Factor.jl

@@ -57,6 +57,20 @@ function PriorPose2Data(;Z::Distribution = FullNormal([0.0, 0.0, 0.0], diagm([0.
     return data
 end
 
+
+"""
+$(SIGNATURES)
+Create a prior factor for a Pose3 with a distribution Z representing (x,y,z,i,j,k) prior information, 
+    e.g. `FullNormal(zeros(6), diagm(0.01*ones(6)))`.
+
+Default value of Z = `FullNormal(zeros(6), diagm(0.01*ones(6)))`.
+"""
+function PriorPose3Data(;Z::Distribution = FullNormal(zeros(6), diagm(0.01*ones(6))), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1], kwargs...)
+    return data
+end
+
+
 """
 $(SIGNATURES)
 Create a prior factor for a Point2 with a distribution Z representing (x,y) prior information, 
@@ -93,6 +107,18 @@ function Pose2Pose2Data(;Z::Distribution = FullNormal([1,0,0.3333*π], diagm([0.
     return data
 end
 
+"""
+$(SIGNATURES)
+Create a Pose3->Pose3 factor with a distribution Z representing the (x,y,theta) relationship
+between the variables, e.g. `FullNormal([1;zeros(5)], diagm(0.01*ones(6)))`.
+
+Default value of Z = `FullNormal(zeros(6), diagm(0.01*ones(6)))`.
+"""
+function Pose3Pose3Data(;Z::Distribution = FullNormal(zeros(6), diagm(0.01*ones(6))), kwargs...)::FactorData
+    data = FactorData(;fnc = ZInferenceType(Z), certainhypo = [1], kwargs...)
+    return data
+end
+
 """
 $(SIGNATURES)
 Create a Pose2->Point2 bearing+range factor with 1D distributions:

src/navability/entities/InferenceTypes.jl

@@ -19,12 +19,18 @@ end
 
 @nvaZInferenceType Prior
 @nvaZInferenceType LinearRelative
-@nvaZInferenceType PriorPose2
-@nvaZInferenceType Pose2Pose2
+
 @nvaZInferenceType PriorPoint2
 @nvaZInferenceType Point2Point2
 @nvaZInferenceType Point2Point2Range
 
+@nvaZInferenceType PriorPose2
+@nvaZInferenceType Pose2Pose2
+
+@nvaZInferenceType PriorPose3
+@nvaZInferenceType Pose3Pose3
+
+
 """
 $(TYPEDEF)
 ZInferenceType is used by many factors as a common inference

src/navability/entities/Variable.jl

@@ -3,70 +3,58 @@ using JSON
 
 DFG_VERSION = "0.18.1";
 
+# FIXME, use dispatch for proper Julian implementation
 _variableTypeConvert = Dict{Symbol, String}(
     :Point2 => "RoME.Point2",
     :Pose2 => "RoME.Pose2",
+    :Pose3 => "RoME.Pose3",
     :ContinuousScalar => "IncrementalInference.ContinuousScalar",
     # TBD - https://github.com/JuliaRobotics/Caesar.jl/discussions/807
     :Position1 => "IncrementalInference.ContinuousScalar",
     #TODO deprecate
     :Pose1 => "IncrementalInference.ContinuousScalar"
 )
 
-struct Variable
+Base.@kwdef struct Variable
     label::String
-    dataEntry::String
-    nstime::String
+    dataEntry::String      = "{}"
+    nstime::String         = "0"
     variableType::String
-    dataEntryType::String
-    ppeDict::String
+    dataEntryType::String  = "{}"
+    ppeDict::String        = "{}"
     solverDataDict::String
-    smallData::String
-    solvable::Int
+    smallData::String      = "{}"
+    solvable::Int          = 1
     tags::String
-    timestamp:: String
-    _version::String
+    timestamp::String      = string(now(Dates.UTC))*"Z" # string(now(TimeZones.localzone()))
+    _version::String       = DFG_VERSION
 end
 
-struct SolverDataDict
-    vecval::Vector{Float64}
+Base.@kwdef struct SolverDataDict
+    vecval::Vector{Float64}     = zeros(0) # FIXME, this is a waste, init can happen on receive side
     dimval::Int
-    vecbw::Vector{Float64}
-    dimbw::Int
-    BayesNetOutVertIDs::Vector{Int}
-    dimIDs::Vector{Int}
-    dims::Int
-    eliminated::Bool
-    BayesNetVertID::String
-    separator::Vector{Int}
+    vecbw::Vector{Float64}      = zeros(dimval)
+    dimbw::Int                  = dimval
+    BayesNetOutVertIDs::Vector{Int}= []
+    dimIDs::Vector{Int}         = collect(range(0,dimval-1, step=1))
+    dims::Int                   = dimval
+    eliminated::Bool            = false
+    BayesNetVertID::String      = "_null"
+    separator::Vector{Int}      = []
     variableType::String
-    initialized::Bool
-    infoPerCoord::Vector{Float64}
-    ismargin::Bool
-    dontmargin::Bool
-    solveInProgress::Int
-    solvedCount::Int
+    initialized::Bool           = false
+    infoPerCoord::Vector{Float64}= zeros(dimval)
+    ismargin::Bool              = false
+    dontmargin::Bool            = false
+    solveInProgress::Int        = 0
+    solvedCount::Int            = 0
     solveKey::String
 end
-function SolverDataDict(variableType::String, solveKey::String, dims::Int)
-    return SolverDataDict(            
-        zeros(dims*100),
-        dims,
-        zeros(dims),
-        dims,   
-        [],
-        collect(range(0,dims-1, step=1)),
-        dims,
-        false,
-        "_null",
-        [],
+function SolverDataDict(variableType::String, solveKey::String, dimval::Int)
+    return SolverDataDict(;
+        vecval=zeros(dimval*100), # FIXME, this is a waste, numerics can happen on receiver side
+        dimval,
         variableType,
-        false,
-        zeros(dims),
-        false,
-        false,
-        0,
-        0,
         solveKey)
 end
 
@@ -75,36 +63,31 @@ Internal utility function to create the correct solver data (variable data)
 given a variable type.
 """
 function _getSolverDataDict(variableType::String, solveKey::String)::SolverDataDict
-    if variableType == "RoME.Point2"
-        return SolverDataDict(variableType, solveKey, 2)
-    end
-    if variableType == "RoME.Pose2"
-        return SolverDataDict(variableType, solveKey, 3)
-    end
+    # TODO impove to Julian dispatch
     if variableType == "IncrementalInference.ContinuousScalar"
         return SolverDataDict(variableType, solveKey, 1)
+    elseif variableType == "RoME.Point2"
+        return SolverDataDict(variableType, solveKey, 2)
+    elseif variableType == "RoME.Pose2"
+        return SolverDataDict(variableType, solveKey, 3)
+    elseif variableType == "RoME.Pose3"
+        return SolverDataDict(variableType, solveKey, 6)
     end
     throw(error("Variable type '$(variableType)' not supported."))
 end
 
-function Variable(label::String, type::Union{String, Symbol}, tags::Vector{String} = ["VARIABLE"], timestamp::String = string(now(Dates.UTC))*"Z")::Variable
+function Variable(label::AbstractString, type::Union{<:AbstractString, Symbol}, tags::AbstractVector{<:AbstractString} = ["VARIABLE"], timestamp::String = string(now(Dates.UTC))*"Z")::Variable
     variableType = type isa Symbol ? get(_variableTypeConvert, type, Nothing) : type
     type == Nothing && error("Variable type '$(type) is not supported")
 
     solverDataDict = Dict("default" => _getSolverDataDict(variableType, "default"))
-    result = Variable(
+    result = Variable(;
         label,
-        "{}",
-        "0",
         variableType,
-        "{}",
-        "{}",
-        json(solverDataDict),
-        "{}",
-        1,
-        json(tags),
-        timestamp,
-        DFG_VERSION
+        # TODO, should not require jsoning, see DFG#867
+        solverDataDict = json(solverDataDict),
+        tags = json(tags),
+        timestamp
     )
     return result
 end