Variables in Caesar.jl
You can check for the latest variable types by running the following in your terminal:
using RoME, Caesar
subtypes(IncrementalInference.InferenceVariable)
IncrementalInference.getCurrentWorkspaceVariables()More variables and factors exists, but are not yet included in the standard library. It is fairly straight forward to build your own out-of-library factors, see page Creating New Variables and Factors for more details.
Default variables in IncrementalInference
IncrementalInference.ContinuousScalar — Typestruct ContinuousScalar <: InferenceVariableMost basic continuous scalar variable in a ::DFG.AbstractDFG object.
DevNotes
- TODO Consolidate with ContinuousEuclid{1}
IncrementalInference.ContinuousEuclid — TypeContinuousEuclid{N}Continuous Euclidean variable of dimension N.
2D Variables
The current variables types are:
RoME.Point2 — Typestruct Point2 <: InferenceVariableXY Euclidean manifold variable node softtype.
RoME.Pose2 — Typestruct Pose2 <: InferenceVariablePose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM.
RoME.DynPoint2 — Typestruct DynPoint2 <: InferenceVariableDynamic point in 2D space with velocity components: x, y, dx/dt, dy/dt
RoME.DynPose2 — Typestruct DynPose2 <: InferenceVariableDynamic pose variable with velocity components: x, y, theta, dx/dt, dy/dt
3D Variables
RoME.Point3 — Typestruct Point3 <: InferenceVariableXYZ Euclidean manifold variable node softtype.
Example
p3 = Point3()RoME.Pose3 — Typestruct Pose3 <: InferenceVariablePose3 is currently a Euler angle mechanization of three Euclidean translations and three Circular rotation.
Future:
- Work in progress on AMP3D for proper non-Euler angle on-manifold operations.
- TODO the AMP upgrade is aimed at resolving 3D to Quat/SE3/SP3 – current Euler angles will be replaced
RoME.InertialPose3 — Typemutable struct InertialPose3 <: AbstractRelativeRootsInertial Odometry version of preintegration procedure and used as a factor between InertialPose3 types for inertial navigation in factor graphs.
Please open an issue with JuliaRobotics/RoME.jl for specific requests, problems, or suggestions. Contributions are also welcome. There might be more variable types in Caesar/RoME/IIF not yet documented here.
Factors in Caesar.jl
You can check for the latest factor types by running the following in your terminal:
using RoME, Caesar
println("- Singletons (priors): ")
println.(sort(string.(subtypes(IIF.AbstractPrior))));
println("- Pairwise (variable constraints): ")
println.(sort(string.(subtypes(IIF.AbstractRelativeRoots))));
println("- Pairwise (variable minimization constraints): ")
println.(sort(string.(subtypes(IIF.AbstractRelativeMinimize))));Priors (Absolute Data)
Defaults in IncrementalInference.jl:
IncrementalInference.Prior — Typestruct Prior{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractPriorDefault prior on all dimensions of a variable node in the factor graph. Prior is not recommended when non-Euclidean dimensions are used in variables.
IncrementalInference.PartialPrior — Typestruct PartialPrior{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, P<:Tuple} <: AbstractPriorPartial prior belief (absolute data) on any variable, given <:SamplableBelief and which dimensions of the intended variable.
Some of the most common priors (unary factors) in Caesar.jl/RoME.jl include:
RoME.PriorPolar — Typemutable struct PriorPolar{T1<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, T2<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractPriorPrior belief on any Polar related variable.
RoME.PriorPoint2 — Typemutable struct PriorPoint2{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractPriorDirection observation information of a Point2 variable.
RoME.PriorPose2 — Typemutable struct PriorPose2{T} <: AbstractPriorIntroduce direct observations on all dimensions of a Pose2 variable:
Example:
PriorPose2( MvNormal([10; 10; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2))) )RoME.PriorPoint3 — Typemutable struct PriorPoint3{T} <: AbstractPriorDirection observation information of a Point3 variable.
RoME.PriorPose3 — Typemutable struct PriorPose3{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractPriorDirect observation information of Pose3 variable type.
Relative Likelihoods (Relative Data)
Defaults in IncrementalInference.jl:
IncrementalInference.LinearRelative — Typestruct LinearRelative{N, T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsDefault linear offset between two scalar variables.
\[X_2 = X_1 + η_Z\]
Existing n-ary factors in Caesar.jl/RoME.jl/IIF.jl include:
RoME.PolarPolar — Typemutable struct PolarPolar{T1<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, T2<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsLinear offset factor of IIF.SamplableBelief between two Polar variables.
RoME.Point2Point2 — Typemutable struct Point2Point2{D<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsRoME.Pose2Point2 — Typestruct Pose2Point2{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeBearing and Range constraint from a Pose2 to Point2 variable.
RoME.Pose2Point2Bearing — Typestruct Pose2Point2Bearing{B<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeSingle dimension bearing constraint from Pose2 to Point2 variable.
RoME.Pose2Point2BearingRange — Typemutable struct Pose2Point2BearingRange{B<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, R<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeBearing and Range constraint from a Pose2 to Point2 variable.
RoME.Pose2Point2Range — Typemutable struct Pose2Point2Range{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeRange only measurement from Pose2 to Point2 variable.
RoME.Pose2Pose2 — Typestruct Pose2Pose2{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsRigid transform between two Pose2's, assuming (x,y,theta).
Related
Pose3Pose3, Point2Point2, MutablePose2Pose2Gaussian, DynPose2, InertialPose3
RoME.DynPoint2VelocityPrior — Typemutable struct DynPoint2VelocityPrior{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractPriorRoME.DynPoint2DynPoint2 — Typemutable struct DynPoint2DynPoint2{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsRoME.VelPoint2VelPoint2 — Typemutable struct VelPoint2VelPoint2{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeRoME.Point2Point2Velocity — Typemutable struct Point2Point2Velocity{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeRoME.DynPose2VelocityPrior — Typemutable struct DynPose2VelocityPrior{T1, T2} <: AbstractPriorRoME.VelPose2VelPose2 — Typemutable struct VelPose2VelPose2{T1<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, T2<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizeRoME.DynPose2Pose2 — Typemutable struct DynPose2Pose2{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsRoME.Pose3Pose3 — Typemutable struct Pose3Pose3{T<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeRootsRigid transform factor between two Pose3 compliant variables.
RoME.PriorPose3ZRP — Typemutable struct PriorPose3ZRP{T1, T2} <: AbstractPriorPartial prior belief on Z, Roll, and Pitch of a Pose3.
RoME.PartialPriorRollPitchZ — Typemutable struct PartialPriorRollPitchZ{T1, T2} <: AbstractPriorPartial prior belief on Roll Pitch and Z of a Pose3 variable.
RoME.PartialPose3XYYaw — Typemutable struct PartialPose3XYYaw{T1<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, T2<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizePartial factor between XY and Yaw of two Pose3 variables.
To be deprecated: use Pose3Pose3XYYaw instead.
RoME.Pose3Pose3XYYaw — Typemutable struct Pose3Pose3XYYaw{T1<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}, T2<:Union{AliasingScalarSampler, BallTreeDensity, ManifoldKernelDensity, Distribution, IncrementalInference.FluxModelsDistribution}} <: AbstractRelativeMinimizePartial factor between XY and Yaw of two Pose3 variables.
Extending Caesar with New Variables and Factors
A question that frequently arises is how to design custom variables and factors to solve a specific type of graph. One strength of Caesar is the ability to incorporate new variables and factors at will. Please refer to Adding Factors for more information on creating your own factors.