       Pose2(1, 0, -.pi / 4) // turn right
     ]
     
+    // Create the graph itself
     var graph = FactorGraph()
     graph.store(PriorFactor(x1, z1)) // prior
     graph.store(PriorFactor(x1, z1))

  let forwardMove = Pose2(1,0,0)
  let (z1, z2, z3) = (Pose2(10, 0, 0),Pose2(11, 0, 0),Pose2(12, 0, 0))
  var expectedTrackingError : Double = 0.0
  override func setUp() {
    super.setUp()
    expectedTrackingError = 2 * origin.localCoordinate(z1).squaredNorm
      +  origin.localCoordinate(z2).squaredNorm
      +  origin.localCoordinate(z3).squaredNorm
      +  2 * origin.localCoordinate(forwardMove).squaredNorm
  }
  func createTrackingFactorGraph() -> (FactorGraph, VariableAssignments) {
    var variables = VariableAssignments()
    let x1 = variables.store(origin)
    let x2 = variables.store(origin)
    let x3 = variables.store(origin)
    
    var graph = FactorGraph()
    graph.store(PriorFactor(x1, z1)) // prior
    graph.store(PriorFactor(x1, z1))

    return (graph, variables)
  }
  
  /// Just a helper for debugging
  func printLabels(_ variables : VariableAssignments) {
    print(variables[TypedID<Int>(0)])
    print(variables[TypedID<Int>(1)])
