How to use the gtsam.ISAM2 function in gtsam

def initialize(data, truth, options):
    # Initialize iSAM
    params = gtsam.ISAM2Params()
    if options.alwaysRelinearize:
        params.setRelinearizeSkip(1)
    isam = gtsam.ISAM2(params=params)

    # Add constraints/priors
    # TODO: should not be from ground truth!
    newFactors = gtsam.NonlinearFactorGraph()
    initialEstimates = gtsam.Values()
    for i in range(2):
        ii = symbol(ord('x'), i)
        if i == 0:
            if options.hardConstraint:  # add hard constraint
                newFactors.add(
                    gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
            else:
                newFactors.add(
                    gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
                                           data.noiseModels.posePrior))
        initialEstimates.insert(ii, truth.cameras[i].pose())

points = SFMdata.createPoints()

    # Create the set of ground-truth poses
    poses = SFMdata.createPoses(K)

    # Create an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
    # to maintain proper linearization and efficient variable ordering, iSAM2
    # performs partial relinearization/reordering at each step. A parameter
    # structure is available that allows the user to set various properties, such
    # as the relinearization threshold and type of linear solver. For this
    # example, we we set the relinearization threshold small so the iSAM2 result
    # will approach the batch result.
    parameters = gtsam.ISAM2Params()
    parameters.setRelinearizeThreshold(0.01)
    parameters.setRelinearizeSkip(1)
    isam = gtsam.ISAM2(parameters)

    # Create a Factor Graph and Values to hold the new data
    graph = gtsam.NonlinearFactorGraph()
    initial_estimate = gtsam.Values()

    #  Loop over the different poses, adding the observations to iSAM incrementally
    for i, pose in enumerate(poses):

        # Add factors for each landmark observation
        for j, point in enumerate(points):
            camera = gtsam.PinholeCameraCal3_S2(pose, K)
            measurement = camera.project(point)
            graph.push_back(gtsam.GenericProjectionFactorCal3_S2(
                measurement, measurement_noise, X(i), L(j), K))

        # Add an initial guess for the current pose

pose_0 = camera.pose()

    # Create the set of ground-truth landmarks and poses
    angular_velocity = math.radians(180)  # rad/sec
    delta_t = 1.0/18  # makes for 10 degrees per step

    angular_velocity_vector = vector3(0, -angular_velocity, 0)
    linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
    scenario = gtsam.ConstantTwistScenario(
        angular_velocity_vector, linear_velocity_vector, pose_0)

    # Create a factor graph
    newgraph = gtsam.NonlinearFactorGraph()

    # Create (incremental) ISAM2 solver
    isam = gtsam.ISAM2()

    # Create the initial estimate to the solution
    # Intentionally initialize the variables off from the ground truth
    initialEstimate = gtsam.Values()

    # Add a prior on pose x0. This indirectly specifies where the origin is.
    # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
    noise = gtsam.noiseModel_Diagonal.Sigmas(
        np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
    newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))

    # Add imu priors
    biasKey = gtsam.symbol(ord('b'), 0)
    biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
    biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
                                              biasnoise)

def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1):
        """ priorMean and priorCov should be in 1 dimensional array
        """

        # init the graph
        self.graph = gtsam.NonlinearFactorGraph()

        # init the iSam2 solver
        parameters = gtsam.ISAM2Params()
        parameters.setRelinearizeThreshold(relinearizeThreshold)
        parameters.setRelinearizeSkip(relinearizeSkip)
        self.isam = gtsam.ISAM2(parameters)

        # init container for initial values
        self.initialValues = gtsam.Values()

        # setting the current position
        self.currentKey = 1

        # current estimate
        self.currentEst = False
        self.currentPose = [0,0,0]

        return