     var opt = LM()
     try! opt.optimize(graph: graph, initial: &variables)
     
-    // check number of factor types
-    print(variables[TypedID<Pose2, Int>(0)])
-    print(variables[TypedID<Pose2, Int>(1)])
-    print(variables[TypedID<Pose2, Int>(2)])
-  }
-  
-  func createSwitchingFactorGraph() -> (NewFactorGraph, VariableAssignments) {
+    // print
+    printPoses(variables)
+  }
+  
+  func createSwitchingFactorGraph() -> (FactorGraph, VariableAssignments) {
     var variables = VariableAssignments()
     let x1 = variables.store(origin)
     let x2 = variables.store(origin)

    var opt = LM()
    try! opt.optimize(graph: graph, initial: &variables)
    
    // check results
    print(variables[TypedID<Pose2, Int>(0)])
    print(variables[TypedID<Pose2, Int>(1)])
    print(variables[TypedID<Pose2, Int>(2)])
    
    // Create initial state for MCMC sampler
    let current_state = variables
    

    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)
