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()
Note

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.ContinuousScalarType
struct ContinuousScalar <: InferenceVariable

Most basic continuous scalar variable in a ::DFG.AbstractDFG object.

DevNotes

  • TODO Consolidate with ContinuousEuclid{1}

2D Variables

The current variables types are:

RoME.Point2Type
struct Point2 <: InferenceVariable

XY Euclidean manifold variable node softtype.

RoME.Pose2Type
struct Pose2 <: InferenceVariable

Pose2 is a SE(2) mechanization of two Euclidean translations and one Circular rotation, used for general 2D SLAM.

RoME.DynPoint2Type
struct DynPoint2 <: InferenceVariable

Dynamic point in 2D space with velocity components: x, y, dx/dt, dy/dt

RoME.DynPose2Type
struct DynPose2 <: InferenceVariable

Dynamic pose variable with velocity components: x, y, theta, dx/dt, dy/dt

Note

  • The SE2E2_Manifold definition used currently is a hack to simplify the transition to Manifolds.jl, see #244

3D Variables

RoME.Point3Type
struct Point3 <: InferenceVariable

XYZ Euclidean manifold variable node softtype.

Example

p3 = Point3()
RoME.Pose3Type
struct Pose3 <: InferenceVariable

Pose3 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.InertialPose3Type
mutable struct InertialPose3 <: AbstractRelativeRoots

Inertial Odometry version of preintegration procedure and used as a factor between InertialPose3 types for inertial navigation in factor graphs.

Note

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.PriorType
struct Prior{T<:SamplableBelief} <: AbstractPrior

Default 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.PartialPriorType
struct PartialPrior{T<:SamplableBelief, P<:Tuple} <: AbstractPrior

Partial 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.PriorPolarType
mutable struct PriorPolar{T1<:SamplableBelief, T2<:SamplableBelief} <: AbstractPrior

Prior belief on any Polar related variable.

RoME.PriorPoint2Type
mutable struct PriorPoint2{T<:SamplableBelief} <: AbstractPrior

Direction observation information of a Point2 variable.

RoME.PriorPose2Type
mutable struct PriorPose2{T} <: AbstractPrior

Introduce 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.PriorPoint3Type
mutable struct PriorPoint3{T} <: AbstractPrior

Direction observation information of a Point3 variable.

RoME.PriorPose3Type
mutable struct PriorPose3{T<:SamplableBelief} <: AbstractPrior

Direct observation information of Pose3 variable type.

Relative Likelihoods (Relative Data)

Defaults in IncrementalInference.jl:

IncrementalInference.LinearRelativeType
struct LinearRelative{N, T<:SamplableBelief} <: AbstractRelativeRoots

Default linear offset between two scalar variables.

\[X_2 = X_1 + η_Z\]

Existing n-ary factors in Caesar.jl/RoME.jl/IIF.jl include:

RoME.PolarPolarType
mutable struct PolarPolar{T1<:SamplableBelief, T2<:SamplableBelief} <: AbstractRelativeRoots

Linear offset factor of IIF.SamplableBelief between two Polar variables.

RoME.Point2Point2Type
mutable struct Point2Point2{D<:SamplableBelief} <: AbstractRelativeRoots
RoME.Pose2Point2Type
struct Pose2Point2{T<:SamplableBelief} <: AbstractRelativeMinimize

Bearing and Range constraint from a Pose2 to Point2 variable.

RoME.Pose2Point2BearingType
struct Pose2Point2Bearing{B<:SamplableBelief} <: AbstractRelativeMinimize

Single dimension bearing constraint from Pose2 to Point2 variable.

RoME.Pose2Point2BearingRangeType
mutable struct Pose2Point2BearingRange{B<:SamplableBelief, R<:SamplableBelief} <: AbstractRelativeMinimize

Bearing and Range constraint from a Pose2 to Point2 variable.

RoME.Pose2Point2RangeType
mutable struct Pose2Point2Range{T<:SamplableBelief} <: AbstractRelativeMinimize

Range only measurement from Pose2 to Point2 variable.

RoME.VelPose2VelPose2Type
mutable struct VelPose2VelPose2{T1<:SamplableBelief, T2<:SamplableBelief} <: AbstractRelativeMinimize
RoME.DynPose2Pose2Type
mutable struct DynPose2Pose2{T<:SamplableBelief} <: AbstractRelativeRoots
RoME.Pose3Pose3Type
mutable struct Pose3Pose3{T<:SamplableBelief} <: AbstractRelativeRoots

Rigid transform factor between two Pose3 compliant variables.

RoME.PriorPose3ZRPType
mutable struct PriorPose3ZRP{T1, T2} <: AbstractPrior

Partial prior belief on Z, Roll, and Pitch of a Pose3.

RoME.PartialPriorRollPitchZType
mutable struct PartialPriorRollPitchZ{T1, T2} <: AbstractPrior

Partial prior belief on Roll Pitch and Z of a Pose3 variable.

RoME.PartialPose3XYYawType
mutable struct PartialPose3XYYaw{T1<:SamplableBelief, T2<:SamplableBelief} <: AbstractRelativeMinimize

Partial factor between XY and Yaw of two Pose3 variables.

To be deprecated: use Pose3Pose3XYYaw instead.

RoME.Pose3Pose3XYYawType
mutable struct Pose3Pose3XYYaw{T1<:SamplableBelief, T2<:SamplableBelief} <: AbstractRelativeMinimize

Partial 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.