# Building and Solving 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 add factors and variables to a graph. This section discusses how to do that in Caesar.

The following sections discuss the steps required to construct a graph and solve it:

- Initialing the Factor Graph
- Adding Variables and Factors to the Graph
- Solving the Graph
- Informing the Solver About Ready Data

## Familiar Canonical Factor Graphs

Starting with a shortcut to just quickly getting a small predefined *canonical* graph containing a few variables and factors can be done with (try tab-completion in the REPL):

```
fg = generateCanonicalFG_Kaess()
fg = generateCanonicalFG_LineStep()
fg = generateCanonicalFG_Hexagonal()
fg = generateCanonicalFG_Circle()
```

Any one of these function *generate* a standard factor graph object that is useful for orientation, testing, learning, or validation. You can generate any of these factor graphs at any time, for example when quickly wanting to test some idea midway through building a more sophisiticated `fg`

, you might just want to quickly do:

`fg_ = generateCanonicalFG_Hexagonal()`

and then work with `fg_`

to try out something risky.

## Building a new Graph

The first step is to model the data (using the most appropriate *factors*) among *variables* of interest. To start model, first create a *distributed factor graph object*:

```
# start with an empty factor graph object
fg = initfg()
```

`IncrementalInference.initfg`

— Function```
initfg()
initfg(dfg; sessionname, robotname, username, cloudgraph)
```

Initialize an empty in-memory DistributedFactorGraph `::DistributedFactorGraph`

object.

## Variables

Variables (a.k.a. poses or states in navigation lingo) are created with the `addVariable!`

fucntion call.

```
# Add the first pose :x0
addVariable!(fg, :x0, Pose2)
# Add a few more poses
for i in 1:10
addVariable!(fg, Symbol("x$(i)"), Pose2)
end
```

Variables contain a label, a data type (e.g. in 2D `RoME.Point2`

or `RoME.Pose2`

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

`DistributedFactorGraphs.addVariable!`

— Function```
addVariable!(dfg, label, varTypeU; N, solvable, timestamp, nanosecondtime, dontmargin, labels, tags, smalldata, checkduplicates, initsolvekeys)
```

Add a variable node `label::Symbol`

to `dfg::AbstractDFG`

, as `varType<:InferenceVariable`

.

**Notes**

- keyword
`nanosecondtime`

is experimental and intended as the whole subsection portion – i.e. accurateTime = (timestamp MOD second) + Nanosecond

**Example**

```
fg = initfg()
addVariable!(fg, :x0, Pose2)
```

Add a DFGVariable to a DFG.

`DistributedFactorGraphs.deleteVariable!`

— Function```
deleteVariable!(dfg, label)
```

Delete a DFGVariable from the DFG using its label.

```
deleteVariable!(dfg, variable)
```

Delete a referenced DFGVariable from the DFG.

Notes

- Returns
`Tuple{AbstractDFGVariable, Vector{<:AbstractDFGFactor}}`

### Initializing Variables

The MM-iSAMv2 algorithm uses one of two approaches to automatically initialize variables. The `initManual!`

function can be used if you wish to overwrite or pre-empt this initialization.

`IncrementalInference.initManual!`

— Function```
initManual!(variable::DFGVariable, ptsArr::Union{var"#s80", var"#s79"} where {var"#s80"<:BallTreeDensity, var"#s79"<:ManifoldKernelDensity})
```

Method to manually initialize a variable using a set of points.

Notes

- Disable automated graphinit on `addFactor!(fg, ...; graphinit=false)
- any un-initialized variables will automatically be initialized by
`solveTree!`

- any un-initialized variables will automatically be initialized by

Example:

```
# some variable is added to fg
addVariable!(fg, :somepoint3, ContinuousEuclid{2})
# data is organized as (row,col) == (dimension, samples)
pts = randn(2,100)
initManual!(fg, :somepoint3, pts)
# manifold management should be done automatically.
# note upgrades are coming to consolidate with Manifolds.jl, see RoME #244
## it is also possible to initManual! by using existing factors, e.g.
initManual!(fg, :x3, [:x2x3f1])
```

DevNotes

- TODO better document graphinit and treeinit.

## Factors

Factors are algebraic relationships between variables based on data cues such as sensor measurements. Examples of factors are absolute (pre-resolved) GPS readings (unary factors/priors) and odometry changes between pose variables. All factors encode a stochastic measurement (measurement + error), such as below, where a generic `Prior`

belief is add to `x0`

(using the `addFactor!`

call) as a normal distribution centered around `[0,0,0]`

.

`DistributedFactorGraphs.addFactor!`

— FunctionAdd factor with user defined type <: FunctorInferenceType to the factor graph object. Define whether the automatic initialization of variables should be performed. Use order sensitive `multihypo`

keyword argument to define if any variables are related to data association uncertainty.

Experimental

`inflation`

, to better disperse kernels before convolution solve, see IIF #1051.

Add a DFGFactor to a DFG.

```
addFactor!(dfg, factor)
```

```
addFactor!(dfg, variables, factor)
```

```
addFactor!(dfg, variableLabels, factor)
```

`DistributedFactorGraphs.deleteFactor!`

— FunctionDelete a DFGFactor from the DFG using its label.

```
deleteFactor!(dfg, factor)
```

Delete the referened DFGFactor from the DFG.

### Priors

```
# Add at a fixed location Prior to pin :x0 to a starting location (0,0,pi/6.0)
addFactor!(fg, [:x0], PriorPose2( MvNormal([0; 0; pi/6.0], Matrix(Diagonal([0.1;0.1;0.05].^2)) )))
```

### Factors Between Variables

```
# Add odometry indicating a zigzag movement
for i in 1:10
pp = Pose2Pose2(MvNormal([10.0;0; (i % 2 == 0 ? -pi/3 : pi/3)], Matrix(Diagonal([0.1;0.1;0.1].^2))))
addFactor!(fg, [Symbol("x$(i-1)"); Symbol("x$(i)")], pp )
end
```

#### [OPTIONAL] Understanding Internal Factor Naming Convention

The factor name used by Caesar is automatically generated from

`addFactor!(fg, [:x0; :x1],...)`

will create a factor with name `:x0x1f1`

When you were to add a another factor betweem `:x0`

, `:x1`

:

`addFactor!(fg, [:x0; :x1],...)`

will create a second factor with the name `:x0x1f2`

.

### Adding Tags

It is possible to add `tags`

to variables and factors that make later graph management tasks easier, e.g.:

`addVariable!(fg, :l7_3, Pose2, tags=[:APRILTAG; :LANDMARK])`

### Drawing the Factor Graph

Once you have a graph, you can visualize the graph as follows (beware though if the fg object is large):

```
# requires `sudo apt-get install graphviz
drawGraph(fg, show=true)
```

By setting `show=true`

, the application `evince`

will be called to show the `fg.pdf`

file that was created using *GraphViz*. A `GraphPlot.jl`

visualization engine is also available.

```
using GraphPlot
dfgplot(fg)
```

`IncrementalInference.drawGraph`

— Function```
drawGraph(fgl; viewerapp, filepath, engine, show)
```

Draw and show the factor graph `<:AbstractDFG`

via system graphviz and xdot app.

Notes

- Requires system install on Linux of
`sudo apt-get install xdot`

- Should not be calling outside programs.
- Need long term solution
- DFG's
`toDotFile`

a better solution – view with`xdot`

application. - also try
`engine={"sfdp","fdp","dot","twopi","circo","neato"}`

Notes:

- Calls external system application
`xdot`

to read the`.dot`

file format`toDot(fg,file=...); @async run(`xdot file.dot`)`

Related

drawGraphCliq, `drawTree`

, printCliqSummary, spyCliqMat

For more details, see the DFG docs on Drawing Graphs.

## When to Instantiate Poses (i.e. new Variables in Factor Graph)

Consider a robot traversing some area while exploring, localizing, and wanting to find strong loop-closure features for consistent mapping. The creation of new poses and landmark variables is a trade-off in computational complexity and marginalization errors made during factor graph construction. Common triggers for new poses are:

- Time-based trigger (eg. new pose a second or 5 minutes if stationary)
- Distance traveled (eg. new pose every 0.5 meters)
- Rotation angle (eg. new pose every 15 degrees)

Computation will progress faster if poses and landmarks are very sparse. To extract the benefit of dense reconstructions, one approach is to use the factor graph as sparse index in history about the general progression of the trajectory and use additional processing from dense sensor data for high-fidelity map reconstructions. Either interpolations, or better direct reconstructions from inertial data can be used for dense reconstruction.

For completeness, one could also re-project the most meaningful measurements from sensor measurements between pose epochs as though measured from the pose epoch. This approach essentially marginalizes the local dead reckoning drift errors into the local interpose re-projections, but helps keep the pose count low.

In addition, see fixed-lag discussion for limiting during inference the number of fluid variables manually to a user desired count.

## Which Variables and Factors to use

See the next page on available variables and factors