Variables in Caesar.jl

You can check for the latest variable types by running the following in your terminal:

using RoME, Caesar

subtypes(IIF.InferenceVariable)

# variables already available
IIF.getCurrentWorkspaceVariables()

# factors already available
IIF.getCurrentWorkspaceFactors()

The variables and factors in Caesar should be sufficient for a variety of robotic applications, however, users can easily extend the framework (without changing the core code). This can even be done out-of-library at runtime after a construction of a factor graph has started! See Custom Variables and Custom Factors for more details.

Basic Variables

Default variables in IncrementalInference

2D Variables

The current variables types are:

RoME.Point2Type
struct Point2 <: InferenceVariable

XY Euclidean manifold variable node softtype.

source
RoME.Pose2Type
struct Pose2 <: InferenceVariable

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

source
RoME.DynPoint2Type
struct DynPoint2 <: InferenceVariable

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

source
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
  • Replaced SE2E2_Manifold hack with ProductManifold(SpecialEuclidean(2), TranslationGroup(2)), confirm if it is correct.
source

3D Variables

RoME.Point3Type
struct Point3 <: InferenceVariable

XYZ Euclidean manifold variable node softtype.

Example

p3 = Point3()
source
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
source
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.

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

Notes

  • If using AMP.ManifoldKernelDensity, don't double partial. Only define the partial in this PartialPrior container.
    • Future TBD, consider using AMP.getManifoldPartial for more general abstraction.
source

Some of the most common priors (unary factors) in Caesar.jl/RoME.jl include:

RoME.PriorPolarType
struct PriorPolar{T1<:(SamplableBelief), T2<:(SamplableBelief)} <: AbstractPrior

Prior belief on any Polar related variable.

source
RoME.PriorPoint2Type
struct PriorPoint2{T<:(SamplableBelief)} <: AbstractPrior

Direction observation information of a Point2 variable.

source
RoME.PriorPose2Type
struct PriorPose2{T<:(SamplableBelief)} <: 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))) )
source
RoME.PriorPoint3Type
struct PriorPoint3{T} <: AbstractPrior

Direction observation information of a Point3 variable.

source
RoME.PriorPose3Type
struct PriorPose3{T<:(SamplableBelief)} <: AbstractPrior

Direct observation information of Pose3 variable type.

source

Relative Likelihoods (Relative Data)

Defaults in IncrementalInference.jl:

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

RoME.PolarPolarType
struct PolarPolar{T1<:(SamplableBelief), T2<:(SamplableBelief)} <: AbstractRelativeMinimize

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

source
RoME.Pose2Point2Type
struct Pose2Point2{T<:(SamplableBelief)} <: AbstractManifoldMinimize

Bearing and Range constraint from a Pose2 to Point2 variable.

source
RoME.Pose2Point2BearingType
struct Pose2Point2Bearing{B<:(SamplableBelief)} <: AbstractManifoldMinimize

Single dimension bearing constraint from Pose2 to Point2 variable.

source
RoME.Pose2Point2BearingRangeType
mutable struct Pose2Point2BearingRange{B<:(SamplableBelief), R<:(SamplableBelief)} <: AbstractManifoldMinimize

Bearing and Range constraint from a Pose2 to Point2 variable.

source
RoME.Pose2Point2RangeType
struct Pose2Point2Range{T<:(SamplableBelief)} <: AbstractManifoldMinimize

Range only measurement from Pose2 to Point2 variable.

source
RoME.Pose2Pose2Type
struct Pose2Pose2{T<:(SamplableBelief)} <: AbstractManifoldMinimize

Rigid transform between two Pose2's, assuming (x,y,theta).

Calcuated as:

\[\begin{aligned} \hat{q}=\exp_pX_m\\ X = \log_q \hat{q}\\ X^i = \mathrm{vee}(q, X) \end{aligned}\]

with: $\mathcal M= \mathrm{SE}(2)$ Special Euclidean group
$p$ and $q$ $\in \mathcal M$ the two Pose2 points
the measurement vector $X_m \in T_p \mathcal M$
and the error vector $X \in T_q \mathcal M$
$X^i$ coordinates of $X$

DevNotes

  • Maybe with Manifolds.jl, {T <: IIF.SamplableBelief, S, R, P}

Related

Pose3Pose3, Point2Point2, MutablePose2Pose2Gaussian, DynPose2, IMUDeltaFactor

source
RoME.Pose3Pose3Type
struct Pose3Pose3{T<:(SamplableBelief)} <: AbstractManifoldMinimize

Rigid transform factor between two Pose3 compliant variables.

source
RoME.PriorPose3ZRPType
struct PriorPose3ZRP{T1<:(SamplableBelief), T2<:(SamplableBelief)} <: AbstractPrior

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

source
RoME.Pose3Pose3XYYawType
struct Pose3Pose3XYYaw{T<:(SamplableBelief)} <: AbstractManifoldMinimize

Partial factor between XY and Yaw of two Pose3 variables.

wR2 = wR1*1R2 = wR1*(1Rψ*Rθ*Rϕ)
wRz = wR1*1Rz
zRz = wRz \ wR(Δψ)

M_R = SO(3)
δ(α,β,γ) = vee(M_R, R_0, log(M_R, R_0, zRz))

M = SE(3)
p0 = identity_element(M)
δ(x,y,z,α,β,γ) = vee(M, p0, log(M, p0, zRz))
source

<!– PartialPose3XYYaw –> <!– PartialPriorRollPitchZ –>

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.