How to use the gtsam.utils.plot function in gtsam

def ISAM2_plot(values, fignum=0):
    """Plot poses."""
    fig = plt.figure(fignum)
    axes = fig.gca(projection='3d')
    plt.cla()

    i = 0
    min_bounds = 0, 0, 0
    max_bounds = 0, 0, 0
    while values.exists(X(i)):
        pose_i = values.atPose3(X(i))
        gtsam_plot.plot_pose3(fignum, pose_i, 10)
        position = pose_i.translation().vector()
        min_bounds = [min(v1, v2) for (v1, v2) in zip(position, min_bounds)]
        max_bounds = [max(v1, v2) for (v1, v2) in zip(position, max_bounds)]
        # max_bounds = min(pose_i.x(), max_bounds[0]), 0, 0
        i += 1

    # draw
    axes.set_xlim3d(min_bounds[0]-1, max_bounds[0]+1)
    axes.set_ylim3d(min_bounds[1]-1, max_bounds[1]+1)
    axes.set_zlim3d(min_bounds[2]-1, max_bounds[2]+1)
    plt.pause(1)

print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))

if args.output is None:
    print("Final Result:\n{}".format(result))
else:
    outputFile = args.output
    print("Writing results to file: ", outputFile)
    graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
    gtsam.writeG2o(graphNoKernel, result, outputFile)
    print ("Done!")

if args.plot:
    resultPoses = gtsam.allPose3s(result)
    for i in range(resultPoses.size()):
        plot.plot_pose3(1, resultPoses.atPose3(i))
    plt.show()

fig = plt.figure(fignum)
    axes = fig.gca(projection='3d')
    plt.cla()

    # Plot points
    # Can't use data because current frame might not see all points
    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
    # gtsam.plot_3d_points(result, [], marginals)
    gtsam_plot.plot_3d_points(fignum, result, 'rx')

    # Plot cameras
    i = 0
    while result.exists(X(i)):
        pose_i = result.atPose3(X(i))
        gtsam_plot.plot_pose3(fignum, pose_i, 10)
        i += 1

    # draw
    axes.set_xlim3d(-40, 40)
    axes.set_ylim3d(-40, 40)
    axes.set_zlim3d(-40, 40)
    plt.pause(1)

def run_example():
    """ Use trajectory interpolation and then trajectory tracking a la Murray
        to move a 3-link arm on a straight line.
    """
    arm = ThreeLinkArm()
    q = np.radians(vector3(30, -30, 45))
    sTt_initial = arm.fk(q)
    sTt_goal = Pose2(2.4, 4.3, math.radians(0))
    poses = trajectory(sTt_initial, sTt_goal, 50)

    fignum = 0
    fig = plt.figure(fignum)
    axes = fig.gca()
    axes.set_xlim(-5, 5)
    axes.set_ylim(0, 10)
    gtsam_plot.plot_pose2(fignum, arm.fk(q))

    for pose in poses:
        sTt = arm.fk(q)
        error = delta(sTt, pose)
        J = arm.jacobian(q)
        q += np.dot(np.linalg.inv(J), error)
        arm.plot(fignum, q)
        plt.pause(0.01)

    plt.pause(10)

def plot(self, fignum, q):
        """ Plot arm.
            Takes figure number, and numpy array of joint angles, in radians.
        """
        fig = plt.figure(fignum)
        axes = fig.gca()

        sXl1 = Pose2(0, 0, math.radians(90))
        t = sXl1.translation()
        p1 = np.array([t.x(), t.y()])
        gtsam_plot.plot_pose2_on_axes(axes, sXl1)

        def plot_line(p, g, color):
            t = g.translation()
            q = np.array([t.x(), t.y()])
            line = np.append(p[np.newaxis], q[np.newaxis], axis=0)
            axes.plot(line[:, 0], line[:, 1], color)
            return q

        l1Zl1 = Pose2(0, 0, q[0])
        l1Xl2 = Pose2(self.L1, 0, 0)
        sTl2 = compose(sXl1, l1Zl1, l1Xl2)
        p2 = plot_line(p1, sTl2, 'r-')
        gtsam_plot.plot_pose2_on_axes(axes, sTl2)

        l2Zl2 = Pose2(0, 0, q[1])
        l2Xl3 = Pose2(self.L2, 0, 0)

print("\nInitial Estimate:\n{}".format(initial))

# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))

# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 4):
    print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))

fig = plt.figure(0)
for i in range(1, 4):
    gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()

# Do not perform more than N iteration steps
parameters.setMaxIterations(100)
# Create the optimizer ...
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)
# ... and optimize
result = optimizer.optimize()
print("Final Result:\n{}".format(result))

# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 6):
    print("X{} covariance:\n{}\n".format(i, marginals.marginalCovariance(i)))

fig = plt.figure(0)
for i in range(1, 6):
    gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, marginals.marginalCovariance(i))

plt.axis('equal')
plt.show()

Author: Ellon Paiva
    Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
    """

    # Declare an id for the figure
    fignum = 0

    fig = plt.figure(fignum)
    axes = fig.gca(projection='3d')
    plt.cla()

    # Plot points
    # Can't use data because current frame might not see all points
    # marginals = Marginals(isam.getFactorsUnsafe(), isam.calculateEstimate())
    # gtsam.plot_3d_points(result, [], marginals)
    gtsam_plot.plot_3d_points(fignum, result, 'rx')

    # Plot cameras
    i = 0
    while result.exists(X(i)):
        pose_i = result.atPose3(X(i))
        gtsam_plot.plot_pose3(fignum, pose_i, 10)
        i += 1

    # draw
    axes.set_xlim3d(-40, 40)
    axes.set_ylim3d(-40, 40)
    axes.set_zlim3d(-40, 40)
    plt.pause(1)

l1Xl2 = Pose2(self.L1, 0, 0)
        sTl2 = compose(sXl1, l1Zl1, l1Xl2)
        p2 = plot_line(p1, sTl2, 'r-')
        gtsam_plot.plot_pose2_on_axes(axes, sTl2)

        l2Zl2 = Pose2(0, 0, q[1])
        l2Xl3 = Pose2(self.L2, 0, 0)
        sTl3 = compose(sTl2, l2Zl2, l2Xl3)
        p3 = plot_line(p2, sTl3, 'g-')
        gtsam_plot.plot_pose2_on_axes(axes, sTl3)

        l3Zl3 = Pose2(0, 0, q[2])
        l3Xt = Pose2(self.L3, 0, 0)
        sTt = compose(sTl3, l3Zl3, l3Xt)
        plot_line(p3, sTt, 'b-')
        gtsam_plot.plot_pose2_on_axes(axes, sTt)

if args.output is None:
    print("\nFactor Graph:\n{}".format(graph))
    print("\nInitial Estimate:\n{}".format(initial))
    print("Final Result:\n{}".format(result))
else:
    outputFile = args.output
    print("Writing results to file: ", outputFile)
    graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
    gtsam.writeG2o(graphNoKernel, result, outputFile)
    print ("Done!")

if args.plot:
    resultPoses = gtsam.extractPose2(result)
    for i in range(resultPoses.shape[0]):
        plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))
    plt.show()