Building Graphs

Adding Variables and Factors (Building Graphs)

Irrespective of your application - real-time robotics, batch processing of survey data, or really complex multi-hypothesis modeling - you're going to need to need to add factors and variables to a graph. This section discusses how to do that with SlamInDb/Graff:

A Quick Discussion on IsReady

Ideally, we want to solve any updated graph as quickly as possible. We're actually architecting the underlying solver structure to do that the moment new data becomes available, so in an ideal world, there is no such thing as a ready graph that doesn't have a solution yet.

However, in some scenarios you want to incrementally build graphs and then let it solve. The IsReady flag on variables provides you with a means to delay that solving.

In a simple scenario, imagine that you want to add two variables (x1 and x2) and relate them with a shared landmark. You may want to delay solving until you've provided all the information. This can be done with the IsReady flag:

Last note of this: Some methods automatically set IsReady to true (such as the addOdometryMeasurement convenience functions) - this is great for examples and continuous incremental solving. That means that you don't need to worry about it, and when we discuss those methods we'll try indicate which automatically set it.

High-Level Convenience Functions for Adding Data to a Graff

Adding Odometry Data

Adding odometry data creates everything all at once for a standard 2D factor of type (x, y, angle). It creates a new variable (say x2) at the end of the graph, links it to the last variable (x1) via a Pose2Pose2 factor, and updates IsReady flags to true.

Just create the AddOdometryRequest request and fire it off:

deltaMeasurement = [1.0; 1.0; pi/4] # x2's pose is (1, 1) away from x1 and the bearing increased by 45 degrees   
pOdo = Float64[0.1 0 0; 0 0.1 0; 0 0 0.01] # Uncertainty in the measurement is in pOdo along the principal diagonal, i.e. [0.1, 0.1, 0.01]
newOdo = AddOdometryRequest(deltaMeasurement, pOdo)
@show addOdoResponse = addOdometryMeasurement(synchronyConfig, newOdo)

# Above would produce x1 in an empty graph.
# Let's run again to produce x2 - assuming the robot travelled the same delta measurement
@show addOdoResponse = addOdometryMeasurement(synchronyConfig, newOdo)

The result would be the following image if run against an empty session:

Simple Odometry Graph

Adding and Attaching Landmarks

We may have seen the same identifying feature in both x1 and x2 (eg. an AprilTag), and want to represent this information. There is a convenience function addBearingRangeFactor that is used to add the factor between the landmark and the variable.

Technically adding landmarks is a lower-level function (addVariable), but in this scenario we want to show the binding of the landmark to variables, so we need to add a landmark with a addVariable call:

newLandmark = VariableRequest(
  "l1", #The variables label
  "Point2", #The type of variable - in this instance it's a 2D point in space, refer to Variable Types section below for the other variable types
  ["LANDMARK"]) #Labels - we are identifying this as a landmark for readability
response = addVariable(synchronyConfig, newLandmark)

We now create the factors to link x1 to l1, and x2 to l1 respectively. The factors are type specific (in this case, relating a 2D position+angle to a 2D point), and include a distribution capturing the uncertainty. You don't need to make them normal distributions, but that's a discussion for later:

newBearingRangeFactor = BearingRangeRequest("x1", "l1",
                          DistributionRequest("Normal", Float64[0; 0.1]), # A statistical measurement of the bearing from x2 to l1 - normal distribution with 0 mean and 0.1 std
                          DistributionRequest("Normal", Float64[20; 1.0]) # A statistical measurement of the range/distance from x2 to l1 - normal distribution with 0 mean and 0.1 std
                          )
addBearingRangeFactor(synchronyConfig, newBearingRangeFactor)

We can add another one between x2 and l1:

newBearingRangeFactor = BearingRangeRequest("x2", "l1",
                          DistributionRequest("Normal", Float64[-pi/4; 0.1]), # A statistical measurement of the bearing from x1 to l1 - normal distribution with 0 mean and 0.1 std
                          DistributionRequest("Normal", Float64[18; 1.0]) # A statistical measurement of the range/distance from x1 to l1 - normal distribution with 0 mean and 0.1 std
                          )
addBearingRangeFactor(synchronyConfig, newBearingRangeFactor)

The graph then becomes:

Odometry Graph with bound landmark

Attaching Sensor Data

Many examples of this can be found in Working with Data

Low-Level Functions for Adding Data to a Graff

_NOTE:_ By default, these are created with isReady = false

Adding Variables

Variables (a.k.a. poses in localization terminology) are created in the same way shown above for the landmark. Variables contain a label, a data type (e.g. a 2D Point or Pose). Note that variables are solved - i.e. they are the product, what you wish to calculate when the solver runs - so you don't provide any measurements when creating them.

For example, we can define x1 as follows:

x1Request = VariableRequest("x1", "Pose2")
response = addVariable(synchronyConfig, x1Request)

x2Request = VariableRequest("x2", "Pose2")
response = addVariable(synchronyConfig, x2Request)

We can also create landmarks in a similar way, and give them an additional label (additional labels can only be 'POSE', 'LANDMARK', or 'EMPTY' for the moment):

newLandmark = VariableRequest("l1", "Point2", ["LANDMARK"])
response = addVariable(synchronyConfig, newLandmark)

NOTE: These are by default created with IsReady set to false. The assumption is that you are building lower-level elements, so you should call putReady once you want these nodes to be solved.

Variable Types

If you have installed RoME, you can check for the latest variable types with:

using RoME
subtypes(IncrementalInference.InferenceVariable)

The current list of available variable types is:

Adding Factors

_NOTE:_ By default, these are created with isReady = false

Creating Factors with RoME

If you have RoME installed, you can lever the RoME library for creating various factors. To continue the prior example, to create the Pose2->Pose2 odometry relationship:

using RoME
using Distributions

# Our measurements
deltaMeasurement = [1.0; 1.0; pi/4] #Same as above - a (1,1) move with a 45 degree heading change
pOdo = Float64[0.1 0 0; 0 0.1 0; 0 0 0.01]
# Creating the factor body - We are working on making this cleaner
p2p2 = Pose2Pose2(MvNormal(deltaMeasurement, pOdo.^2))
p2p2Conv = convert(PackedPose2Pose2, p2p2)
p2p2Request = FactorRequest(["x0", "x1"], "Pose2Pose2", p2p2Conv)

# Send the request for the x0->x1 link
addFactor(synchronyConfig, p2p2Request)
# Update the request to make the same link between x1 and x2
p2p2Request.variables = ["x1", "x2"]
addFactor(synchronyConfig, p2p2Request)

Now we can add the factors between the variables and the landmark. As above, this is a 2D pose to 2D point+bearing factor, and is built similar to above:

# Lastly, let's add the bearing+range factor between x1 and landmark l1
bearingDist = Normal(-pi/4, 0.1)
rangeDist = Normal(18, 1.0)
p2br2 = Pose2Point2BearingRange(bearingDist, rangeDist)
p2br2Conv = convert(PackedPose2Point2BearingRange, p2br2)
p2br2Request = FactorRequest(["x1", "l1"], "Pose2Point2BearingRange", p2br2Conv)

addFactor(synchronyConfig, p2br2Request)
# Now add the x1->l1 bearing+range factor
p2br2Request.variables = ["x2", "l1"]
addFactor(synchronyConfig, p2br2Request)

Creating Factors Natively

[TODO] In some instances, you are running a parallel local solver, so RoME will be available for factor creation. In other, smaller instances, you may rely solely on the cloud solution. In this case, you need to create factors without pulling in RoME. TBD - Still working on this.

Factor Types

If you have installed RoME, you can check for the latest factor types with:

using RoME
subtypes(IncrementalInference.FunctorPairwise)

The current factor types that you will find in the example are (there are many aside from these):