How to use the gtsam.NonlinearFactorGraph function in gtsam

def BatchFixedLagSmootherExample():
    """
    Runs a batch fixed smoother on an agent with two odometry
    sensors that is simply moving to the
    """

    # Define a batch fixed lag smoother, which uses
    # Levenberg-Marquardt to perform the nonlinear optimization
    lag = 2.0
    smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)

    # Create containers to store the factors and linearization points
    # that will be sent to the smoothers
    new_factors = gtsam.NonlinearFactorGraph()
    new_values = gtsam.Values()
    new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()

    # Create  a prior on the first pose, placing it at the origin
    prior_mean = gtsam.Pose2(0, 0, 0)
    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
    X1 = 0
    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
    new_values.insert(X1, prior_mean)
    new_timestamps.insert(_timestamp_key_value(X1, 0.0))

    delta_time = 0.25
    time = 0.25

    while time <= 3.0:
        previous_key = 1000 * (time - delta_time)

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]

target = gtsam.Point3(0, 0, 0)
    position = gtsam.Point3(radius, 0, 0)
    camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
    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)

from __future__ import print_function

import numpy as np

import gtsam

import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot

# Create noise models
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))

# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()

# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))

# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
# For simplicity, we will use the same noise model for each odometry factor
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))

# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values

def step(data, isam, result, truth, currPoseIndex):
    '''
    Do one step isam update
    @param[in] data: measurement data (odometry and visual measurements and their noiseModels)
    @param[in/out] isam: current isam object, will be updated
    @param[in/out] result: current result object, will be updated
    @param[in] truth: ground truth data, used to initialize new variables
    @param[currPoseIndex]: index of the current pose
    '''
    # iSAM expects us to give it a new set of factors
    # along with initial estimates for any new variables introduced.
    newFactors = gtsam.NonlinearFactorGraph()
    initialEstimates = gtsam.Values()

    # Add odometry
    prevPoseIndex = currPoseIndex - 1
    odometry = data.odometry[prevPoseIndex]
    newFactors.add(
        gtsam.BetweenFactorPose3(
            symbol(ord('x'), prevPoseIndex),
            symbol(ord('x'), currPoseIndex), odometry,
            data.noiseModels.odometry))

    # Add visual measurement factors and initializations as necessary
    for k in range(len(data.Z[currPoseIndex])):
        zij = data.Z[currPoseIndex][k]
        j = data.J[currPoseIndex][k]
        jj = symbol(ord('l'), j)

# Add Imu Factor
            imufac = gtsam.ImuFactor(
                X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
            newgraph.add(imufac)

            # insert new velocity, which is wrong
            initialEstimate.insert(V(i), n_velocity)
            accum.resetIntegration()

        # Incremental solution
        isam.update(newgraph, initialEstimate)
        result = isam.calculateEstimate()
        ISAM2_plot(result)

        # reset
        newgraph = gtsam.NonlinearFactorGraph()
        initialEstimate.clear()

prior.print_('goal angle')
    model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
    key = gtsam.symbol(ord('x'), 1)
    factor = gtsam.PriorFactorRot2(key, prior, model)

    """
    Step 2: Create a graph container and add the factor to it

    Before optimizing, all factors need to be added to a Graph container,
    which provides the necessary top-level functionality for defining a
    system of constraints.

    In this case, there is only one factor, but in a practical scenario,
    many more factors would be added.
    """
    graph = gtsam.NonlinearFactorGraph()
    graph.push_back(factor)
    graph.print_('full graph')

    """
    Step 3: Create an initial estimate

    An initial estimate of the solution for the system is necessary to
    start optimization.  This system state is the "Values" instance,
    which is similar in structure to a dictionary, in that it maps
    keys (the label created in step 1) to specific values.

    The initial estimate provided to optimization will be used as
    a linearization point for optimization, so it is important that
    all of the variables in the graph have a corresponding value in
    this structure.
    """