Adding New Variables and Factors

Creating New Variables and Factors

In most scenarios, the existing variables and factors should be sufficient for most robotics applications. Caesar however, is extensible and allows you to easily incorporate your own variable and factor types for specialized applications.

Considerations

A couple of important points:

Getting Started

We suggest the following design pattern for developing and building new factors:

  1. You have reviewed the variable and factor types available in Caesar, RoME, and IncrementalInference and a new type is required - please see Building and Solving Graphs if you want to review what is currently available
  2. Create a GitHub repository to store the new types
  3. Create your new variable types
  4. Create your new factor types
  5. Implement unit tests to validate the correct operation of the types
  6. Set up your solver to make use the custom types

1.1. This is much easier than it sounds

  1. If the code is public and may be useful to the community, we ask if you could submit an issue against Caesar with information about the new types and the repository. Ideally we'd like to continually improve the core code and fold in community contributions.

The remainder of this section discusses each of these steps.

Reviewing the Existing Types

Please see Building and Solving Graphs to review what variables and factors are currently supported.

[OPTIONAL] Creating a Repository

You can fork the following template repository to construct your own Caesar Variable and Factor Examples.

If this repository is going to be used for development of the new variables/factors as well as for the experiment (i.e. the code that builds the graph and solves it), you should probably start a simple end-to-end test that validates a basic version of your experimental setup (e.g. ):

#### This example is a basic test of the new variables and factors
#### that are added in this repo. The example is derived from
#### the hexagonal test example.

using Caesar, RoME
using Caesar_VariableFactorExamples # Your new variable/factor repository
# Using plotting for experiment validation
using RoMEPlotting

# 1. Init factor graph
#TODO

# 2. Add variables
#TODO

# 3. Add factors
# 3a. Add a new test prior
#TODO
# 3b. Add new types of odometry factors.
#TODO

# 4. Solve graph
batchSolve!(fg)

# 5. Graph solution - assuming that you have this open in Atom.
drawPoses(fg)

Creating New Variables

All variables have to derive from IncrementalInference.InferenceVariable.

What you need to build in the variable:

You can then also add any additional fields that you would like to use for saving state information in variable. Note that these fields must be serializable as both JSON and Protobufs. Although you don't need to validate this, please keep the fields fairly simple and avoid complex structures with optional fields. TBD - provide a compatibility check for serialization and a docpage on it.

In a trivial example of Pose2:

Creating New Factors

All factors inherit from one of the following types, depending on their function:

How do you decide which to use?

TBD: sUsers should start with FunctorPairwiseMinimize, discuss why and when they should promote their factors to FunctorPairwise.

Note: FunctorPairwiseMinimize does not imply that the overall inference algorithm only minimizes an objective function. The Multi-model iSAM algorithm is built around fixed-point analysis. Minimization is used here to locally enforce the residual function.

What you need to build in the new factor:

An example of this is the Pose2Point2BearingRange, which provides a bearing+range relationship between a 2D pose and a 2D point.

Pose2Point2BearingRange Struct

mutable struct Pose2Point2BearingRange{B <: IIF.SamplableBelief, R <: IIF.SamplableBelief} <: IncrementalInference.FunctorPairwise
    bearing::B
    range::R
    Pose2Point2BearingRange{B,R}() where {B,R} = new{B,R}()
    Pose2Point2BearingRange{B,R}(x1::B,x2::R) where {B <: IIF.SamplableBelief,R <: IIF.SamplableBelief} = new{B,R}(x1,x2)
end
# Convenient constructor
Pose2Point2BearingRange(x1::B,x2::R) where {B <: IIF.SamplableBelief,R <: IIF.SamplableBelief} = Pose2Point2BearingRange{B,R}(x1,x2)

Pose2Point2BearingRange Sampler

# Return N samples from the two distributions
function getSample(pp2br::Pose2Point2BearingRange, N::Int=1)
  smpls = zeros(2, N)
  smpls[1,:] = rand(pp2br.bearing, N)[:]
  smpls[2,:] = rand(pp2br.range, N)[:]
  return (smpls,)
end

Pose2Point2BearingRange Residual Function (Functor)

# define the conditional probability constraint
function (pp2br::Pose2Point2BearingRange)(res::Array{Float64},
        userdata::FactorMetadata,
        idx::Int,
        meas::Tuple{Array{Float64,2}},
        xi::Array{Float64,2},
        lm::Array{Float64,2} )
  #
  res[1] = lm[1,idx] - (meas[1][2,idx]*cos(meas[1][1,idx]+xi[3,idx]) + xi[1,idx])
  res[2] = lm[2,idx] - (meas[1][2,idx]*sin(meas[1][1,idx]+xi[3,idx]) + xi[2,idx])
  nothing
end

Pose2Point2BearingRange Packing and Unpacking

The packing structure:

mutable struct PackedPose2Point2BearingRange <: IncrementalInference.PackedInferenceType
    bearstr::String
    rangstr::String
    PackedPose2Point2BearingRange() = new()
    PackedPose2Point2BearingRange(s1::AS, s2::AS) where {AS <: AbstractString} = new(string(s1),string(s2))
end

The packing and unpacking converters (note the use of string and extractdistribution):

function convert(::Type{PackedPose2Point2BearingRange}, d::Pose2Point2BearingRange{B, R}) where {B <: IIF.SamplableBelief, R <: IIF.SamplableBelief}
  return PackedPose2Point2BearingRange(string(d.bearing), string(d.range))
end

function convert(::Type{Pose2Point2BearingRange}, d::PackedPose2Point2BearingRange)
 # where {B <: IIF.SamplableBelief, R <: IIF.SamplableBelief}
  Pose2Point2BearingRange(extractdistribution(d.bearstr), extractdistribution(d.rangstr))
end

Unit Tests

What you should test:

An example of these tests can be seen for the trivial case shown in the example repo ExamplePrior Unit Tests.

Using your Types with the Caesar Solver

As above, as long as you bring your factors into the workspace, you should be able to use them in your experimental setup.

You can validate this with the existence check code in Building and Solving Graphs.

Note: This has been made available as IncrementalInference.getCurrentWorkspaceVariables() and IncrementalInference.getCurrentWorkspaceFactors()in IncrementalInference v0.4.4.

Contributing to Community

We really appreciate any contributions, so if you have developed variables and factors that may be useful to the community, please write up an issue in Caesar.jl with a link to your repo and a short description of the use-case(s).