     let flipAndOptimize = {(x:VariableAssignments) -> VariableAssignments in
       let labelVars = x.storage[ObjectIdentifier(Int.self)]
       let positionVars = x.storage[ObjectIdentifier(Pose2.self)]
-
+      
       // Randomly change one label.
       let i = Int.random(in: 0..<labelVars!.count)
       let id = TypedID<Int, Int>(i)

      +  origin.localCoordinate(z3).squaredNorm
      +  2 * origin.localCoordinate(forwardMove).squaredNorm
  }
  func createTrackingFactorGraph() -> (NewFactorGraph, VariableAssignments) {
    var variables = VariableAssignments()
    let x1 = variables.store(origin)
    let x2 = variables.store(origin)
    let x3 = variables.store(origin)
    
    var graph = NewFactorGraph()
    graph.store(NewPriorFactor2(x1, z1)) // prior
    graph.store(NewPriorFactor2(x1, z1))
    graph.store(NewPriorFactor2(x2, z2))
    graph.store(NewPriorFactor2(x3, z3))
    graph.store(NewBetweenFactor2(x1, x2, forwardMove))
    graph.store(NewBetweenFactor2(x2, x3, forwardMove))
    
    return (graph, variables)
  }
  
  /// Tracking example from Figure 2.a
  func testTrackingExample() {
    // create a factor graph
    var (graph, variables) = createTrackingFactorGraph()
    _ = graph as NewFactorGraph
    
    // check number of factor types
    XCTAssertEqual(graph.storage.count, 2)

    return errorVector(x.head, x.tail.head, x.tail.tail.head)
  }
  
  public typealias Linearization = JacobianFactor<JacobianRows, ErrorVector>
  public func linearized(at x: Variables) -> Linearization {
    let (start, label, end) = (x.head, x.tail.head, x.tail.tail.head)
    let (startEdge, endEdge) = (edges.head, edges.tail.tail.head)
