                  + UVT[2, 0].scalar! * (UVT[0, 1].scalar! * UVT[1, 2].scalar! - UVT[1, 1].scalar! * UVT[0, 2].scalar!))
       
       let R = matmul(matmul(U!, Tensor<Double>(shape: [3], scalars: [1, 1, det]).diagonal()), V!.transposed()).transposed()
-      // ClosestTo finds rotation matrix closest to H in Frobenius sense
+      
       let initRot = Rot3(coordinate:
                           Matrix3Coordinate(Matrix3(R[0, 0].scalar!, R[0, 1].scalar!, R[0, 2].scalar!,
                                                     R[1, 0].scalar!, R[1, 1].scalar!, R[1, 2].scalar!,
                                                     R[2, 0].scalar!, R[2, 1].scalar!, R[2, 2].scalar!))
       )
-      let id = validRot3.store(initRot)
+      
+      // TODO(fan): relies on the assumption of continous and ordered allocation
+      let _ = validRot3.store(initRot)
     }
     return validRot3;
   }
   
+  /// This function computes the rotations
   public func computeOrientationsChordal(graph: FactorGraph, val: VariableAssignments, ids: Array<TypedID<Pose3, Int>>) -> VariableAssignments {
     var relaxedRot3 = VariableAssignments()
     var associations = Dictionary<Int, TypedID<Vector9, Int>>()

    return val
  }
  
  public mutating func GetInitializations(graph: FactorGraph, val: VariableAssignments, ids: Array<TypedID<Pose3, Int>>) -> VariableAssignments {
    var val_copy = val
    anchorId = val_copy.store(Pose3())

    anchorId = TypedID<Pose3, Int>(0)
  }
  
  /// Extract a subgraph of the original graph with only Pose3s.
  public func buildPose3graph(graph: FactorGraph) -> FactorGraph {
    var pose3Graph = FactorGraph()

