예제 #1
0
    def plot_sfm_result(self,result):

        # 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')
        # gtsam_plot.plot_pose3(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, 2)
            i += 1

        # draw
        axes.set_xlim3d(-50, 50)
        axes.set_ylim3d(-50, 50)
        axes.set_zlim3d(-50, 50)
        plt.legend()
        plt.show()
예제 #2
0
def visual_ISAM2_plot(result):
    """
    VisualISAMPlot plots current state of ISAM2 object
    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)
예제 #3
0
def plot_with_result(result,
                     x_axe=30,
                     y_axe=30,
                     z_axe=30,
                     axis_length=2,
                     figure_number=0):
    """plot the result of sfm"""
    # Declare an id for the figure
    fig = plt.figure(figure_number)
    axes = fig.gca(projection='3d')
    plt.cla()
    # Plot points
    gtsam_plot.plot_3d_points(figure_number, result, 'rx')
    # Plot cameras
    i = 0
    while result.exists(X(i)):
        pose_i = result.atPose3(X(i))
        axis_length = axis_length
        gtsam_plot.plot_pose3(figure_number, pose_i, axis_length)
        i += 1

    # draw
    axes.set_xlim3d(-x_axe, x_axe)
    axes.set_ylim3d(-y_axe, y_axe)
    axes.set_zlim3d(-z_axe, z_axe)
    plt.legend()
    plt.show()
예제 #4
0
    def plot_estimate(self, fignum=0):
        """
        VisualISAMPlot plots current state of ISAM2 object
        Author: Ellon Paiva
        Based on MATLAB version by: Duy Nguyen Ta and Frank Dellaert
        """
        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, self.current_estimate, 'rx')

        # Plot cameras
        i = 0
        while self.current_estimate.exists(iSAM2Wrapper.get_key('x', i)):
            pose_i = self.current_estimate.atPose3(iSAM2Wrapper.get_key(
                '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)
        axes.view_init(90, 0)

        plt.pause(.01)
예제 #5
0
def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
    """Plot the SFM results."""
    plot_vals = gtsam.Values()
    for i in range(scene_data.numberCameras()):
        plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
    for j in range(scene_data.numberTracks()):
        plot_vals.insert(P(j), result.atPoint3(P(j)))

    plot.plot_3d_points(0, plot_vals, linespec="g.")
    plot.plot_trajectory(0, plot_vals, title="SFM results")

    plt.show()
예제 #6
0
def plot_with_results(result1,
                      result2,
                      x_axe=30,
                      y_axe=30,
                      z_axe=30,
                      figure_number1=0,
                      figure_number2=1):
    """plot the two results of sfm at the same time"""
    # Declare an id for the figure
    fig = plt.figure(figure_number1)
    axes = fig.gca(projection='3d')
    plt.cla()
    # Plot points
    gtsam_plot.plot_3d_points(figure_number1, result1, 'rx')
    # Plot cameras
    i = 0
    while result1.exists(X(i)):
        pose_i = result1.atPose3(X(i))
        gtsam_plot.plot_pose3(figure_number1, pose_i, 2)
        i += 1

    # draw
    axes.set_xlim3d(-x_axe, x_axe)
    axes.set_ylim3d(-y_axe, y_axe)
    axes.set_zlim3d(-z_axe, z_axe)

    # Declare an id for the figure
    fig = plt.figure(figure_number2)
    axes = fig.gca(projection='3d')
    plt.cla()
    # Plot points
    gtsam_plot.plot_3d_points(figure_number2, result2, 'rx')
    # Plot cameras
    i = 0
    while result2.exists(X(i)):
        pose_i = result2.atPose3(X(i))
        gtsam_plot.plot_pose3(figure_number2, pose_i, 2)
        i += 1

    # draw
    axes.set_xlim3d(-x_axe, x_axe)
    axes.set_ylim3d(-y_axe, y_axe)
    axes.set_zlim3d(-z_axe, z_axe)
    plt.legend()
    plt.show()
예제 #7
0
def main():
    """
    Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).

    Each variable in the system (poses and landmarks) must be identified with a unique key.
    We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
    Here we will use Symbols

    In GTSAM, measurement functions are represented as 'factors'. Several common factors
    have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
    Here we will use Projection factors to model the camera's landmark observations.
    Also, we will initialize the robot at some location using a Prior factor.

    When the factors are created, we will add them to a Factor Graph. As the factors we are using
    are nonlinear factors, we will need a Nonlinear Factor Graph.

    Finally, once all of the factors have been added to our factor graph, we will want to
    solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
    GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
    trust-region method known as Powell's Degleg

    The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
    nonlinear functions around an initial linearization point, then solve the linear system
    to update the linearization point. This happens repeatedly until the solver converges
    to a consistent set of variable values. This requires us to specify an initial guess
    for each variable, held in a Values container.
    """

    # Define the camera calibration parameters
    K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)

    # Define the camera observation noise model
    measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
        2, 1.0)  # one pixel in u and v

    # Create the set of ground-truth landmarks
    points = SFMdata.createPoints()

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

    # Create a factor graph
    graph = NonlinearFactorGraph()

    # Add a prior on pose x1. This indirectly specifies where the origin is.
    # 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
    pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
    factor = PriorFactorPose3(X(0), poses[0], pose_noise)
    graph.push_back(factor)

    # Simulated measurements from each camera pose, adding them to the factor graph
    for i, pose in enumerate(poses):
        camera = PinholeCameraCal3_S2(pose, K)
        for j, point in enumerate(points):
            measurement = camera.project(point)
            factor = GenericProjectionFactorCal3_S2(measurement,
                                                    measurement_noise, X(i),
                                                    L(j), K)
            graph.push_back(factor)

    # Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
    # Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
    # between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
    point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
    factor = PriorFactorPoint3(L(0), points[0], point_noise)
    graph.push_back(factor)
    graph.print_('Factor Graph:\n')

    # Create the data structure to hold the initial estimate to the solution
    # Intentionally initialize the variables off from the ground truth
    initial_estimate = Values()
    for i, pose in enumerate(poses):
        transformed_pose = pose.retract(0.1 * np.random.randn(6, 1))
        initial_estimate.insert(X(i), transformed_pose)
    for j, point in enumerate(points):
        transformed_point = point + 0.1 * np.random.randn(3)
        initial_estimate.insert(L(j), transformed_point)
    initial_estimate.print_('Initial Estimates:\n')

    # Optimize the graph and print results
    params = gtsam.DoglegParams()
    params.setVerbosity('TERMINATION')
    optimizer = DoglegOptimizer(graph, initial_estimate, params)
    print('Optimizing:')
    result = optimizer.optimize()
    result.print_('Final results:\n')
    print('initial error = {}'.format(graph.error(initial_estimate)))
    print('final error = {}'.format(graph.error(result)))

    marginals = Marginals(graph, result)
    plot.plot_3d_points(1, result, marginals=marginals)
    plot.plot_trajectory(1, result, marginals=marginals, scale=8)
    plot.set_axes_equal(1)
    plt.show()