How to use the gtsam.noiseModel_Isotropic.Sigma function in gtsam

To help you get started, we’ve selected a few gtsam examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github borglab / gtsam / cython / gtsam / examples / SFMExample.py View on Github external
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(symbol('x', 0), poses[0], pose_noise)
    graph.push_back(factor)
github borglab / gtsam / cython / gtsam / examples / VisualISAMExample.py View on Github external
def main():
    """
    A structure-from-motion example with landmarks
    - The landmarks form a 10 meter cube
    - The robot rotates around the landmarks, always facing towards the cube
    """

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

    # Define the camera observation noise model
    camera_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 NonlinearISAM object which will relinearize and reorder the variables
    # every "reorderInterval" updates
    isam = NonlinearISAM(reorderInterval=3)

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

    # Loop over the different poses, adding the observations to iSAM incrementally
    for i, pose in enumerate(poses):
github borglab / gtsam / cython / gtsam / utils / visual_data_generator.py View on Github external
def __init__(self, K=gtsam.Cal3_S2(), nrCameras=3, nrPoints=4):
        self.K = K
        self.Z = [x[:] for x in [[gtsam.Point2()] * nrPoints] * nrCameras]
        self.J = [x[:] for x in [[0] * nrPoints] * nrCameras]
        self.odometry = [gtsam.Pose3()] * nrCameras

        # Set Noise parameters
        self.noiseModels = Data.NoiseModels()
        self.noiseModels.posePrior = gtsam.noiseModel_Diagonal.Sigmas(
            np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1]))
        # noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
        #    np.array([0.001,0.001,0.001,0.1,0.1,0.1]))
        self.noiseModels.odometry = gtsam.noiseModel_Diagonal.Sigmas(
            np.array([0.05, 0.05, 0.05, 0.2, 0.2, 0.2]))
        self.noiseModels.pointPrior = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
        self.noiseModels.measurement = gtsam.noiseModel_Isotropic.Sigma(2, 1.0)
github borglab / gtsam / cython / gtsam / examples / VisualISAM2Example.py View on Github external
def visual_ISAM2_example():
    plt.ion()

    # Define the camera calibration parameters
    K = gtsam.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 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()
github borglab / gtsam / cython / gtsam / examples / SFMExample.py View on Github external
factor = PriorFactorPose3(symbol('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 = SimpleCamera(pose, K)
        for j, point in enumerate(points):
            measurement = camera.project(point)
            factor = GenericProjectionFactorCal3_S2(
                measurement, measurement_noise, symbol('x', i), symbol('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(symbol('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):
        r = Rot3.Rodrigues(-0.1, 0.2, 0.25)
        t = Point3(0.05, -0.10, 0.20)
        transformed_pose = pose.compose(Pose3(r, t))
        initial_estimate.insert(symbol('x', i), transformed_pose)
    for j, point in enumerate(points):
        transformed_point = Point3(point.vector() + np.array([-0.25, 0.20, 0.15]))
        initial_estimate.insert(symbol('l', j), transformed_point)
    initial_estimate.print_('Initial Estimates:\n')