How to use gtsam - 10 common examples

def test_PlanarSLAMExample(): 
    print("test_PlanarSLAMExample\n")
    print("=================================")

    # Create a factor graph
    graph = NonlinearFactorGraph()

    # Create the keys we need for this simple example
    x1, x2, x3 = symbol('x', 1), symbol('x', 2), symbol('x', 3)
    l1, l2 = symbol('l', 1), symbol('l', 2)
    
    # Add a prior on pose x1 at the origin. A prior factor consists of a mean
    # and a noise model (covariance matrix)
    prior = Pose2(0.0, 0.0, 0.0) #  prior mean is at origin
    prior_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True) #  30cm std on x,y, 0.1 rad on theta
    graph.add(PriorFactorPose2(x1, prior, prior_noise)) #  add directly to graph
    
    # Add two odometry factors
    odometry = Pose2(2.0, 0.0, 0.0) #  create a measurement for both factors (the same in this case)
    odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True) #  20cm std on x,y, 0.1 rad on theta
    graph.add(BetweenFactorPose2(x1, x2, odometry, odometry_noise))
    graph.add(BetweenFactorPose2(x2, x3, odometry, odometry_noise))

    # Add Range-Bearing measurements to two different landmarks
    # create a noise model for the landmark measurements
    measurement_noise = Diagonal.Sigmas(vec(0.1, 0.2), True) #  0.1 rad std on bearing, 20cm on range

def test_PlanarSLAMExample(): 
    print("test_PlanarSLAMExample\n")
    print("=================================")

    # Create a factor graph
    graph = NonlinearFactorGraph()

    # Create the keys we need for this simple example
    x1, x2, x3 = symbol('x', 1), symbol('x', 2), symbol('x', 3)
    l1, l2 = symbol('l', 1), symbol('l', 2)
    
    # Add a prior on pose x1 at the origin. A prior factor consists of a mean
    # and a noise model (covariance matrix)
    prior = Pose2(0.0, 0.0, 0.0) #  prior mean is at origin
    prior_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True) #  30cm std on x,y, 0.1 rad on theta
    graph.add(PriorFactorPose2(x1, prior, prior_noise)) #  add directly to graph
    
    # Add two odometry factors
    odometry = Pose2(2.0, 0.0, 0.0) #  create a measurement for both factors (the same in this case)
    odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True) #  20cm std on x,y, 0.1 rad on theta
    graph.add(BetweenFactorPose2(x1, x2, odometry, odometry_noise))
    graph.add(BetweenFactorPose2(x2, x3, odometry, odometry_noise))

    # Add Range-Bearing measurements to two different landmarks
    # create a noise model for the landmark measurements

def test_StereoVOExample():
    print("test_StereoVOExample\n")
    print("=================================")

    # Assumptions
    #  - For simplicity this example is in the camera's coordinate frame
    #  - X: right, Y: down, Z: forward
    #  - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
    #  - x1 is fixed with a constraint, x2 is initialized with noisy values
    #  - No noise on measurements
    x1, x2 = symbol('x', 1), symbol('x', 2)
    l1, l2, l3 = symbol('l', 1), symbol('l', 2), symbol('l', 3)

    # Create a factor graph
    graph = NonlinearFactorGraph()

    # add a constraint on the starting pose
    first_pose = Pose3()
    graph.add(NonlinearEqualityPose3(x1, first_pose))

    # Create realistic calibration and measurement noise model
    # format: fx fy skew cx cy baseline
    K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
    stereo_model = Diagonal.Sigmas(vec(1.0, 1.0, 1.0))

    ## Add measurements
    # pose 1
    graph.add(GenericStereoFactor3D(

measurement = camera.project(pointj)
            print measurement
            
            # call add() function to add measurement into a single factor, here we need to add:
            #    1. the 2D measurement
            #    2. the corresponding camera's key
            #    3. camera noise model
            #    4. camera calibration
            smartfactor.add_single(measurement, i, measurement_noise, K)

        # insert the smart factor in the graph
        graph.add(smartfactor)
    
    # 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
    pose_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.3, 0.1, 0.1, 0.1))
    graph.add(PriorFactorPose3(0, poses[0], pose_noise))

    # Because the structure-from-motion problem has a scale ambiguity, the problem is
    # still under-constrained. Here we add a prior on the second pose x1, so this will
    # fix the scale by indicating the distance between x0 and x1.
    # Because these two are fixed, the rest of the poses will be also be fixed.
    graph.add(PriorFactorPose3(1, poses[1], pose_noise)) # add directly to graph

    graph.printf("Factor Graph:\n")

    # Create the initial estimate to the solution
    # Intentionally initialize the variables off from the ground truth
    initialEstimate = Values()
    delta = Pose3(Rot3.rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))

    for i, posei in enumerate(poses):

def test_PlanarSLAMExample(): 
    print("test_PlanarSLAMExample\n")
    print("=================================")

    # Create a factor graph
    graph = NonlinearFactorGraph()

    # Create the keys we need for this simple example
    x1, x2, x3 = symbol('x', 1), symbol('x', 2), symbol('x', 3)
    l1, l2 = symbol('l', 1), symbol('l', 2)
    
    # Add a prior on pose x1 at the origin. A prior factor consists of a mean
    # and a noise model (covariance matrix)
    prior = Pose2(0.0, 0.0, 0.0) #  prior mean is at origin
    prior_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True) #  30cm std on x,y, 0.1 rad on theta
    graph.add(PriorFactorPose2(x1, prior, prior_noise)) #  add directly to graph
    
    # Add two odometry factors
    odometry = Pose2(2.0, 0.0, 0.0) #  create a measurement for both factors (the same in this case)
    odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True) #  20cm std on x,y, 0.1 rad on theta
    graph.add(BetweenFactorPose2(x1, x2, odometry, odometry_noise))
    graph.add(BetweenFactorPose2(x2, x3, odometry, odometry_noise))

    # Add Range-Bearing measurements to two different landmarks
    # create a noise model for the landmark measurements
    measurement_noise = Diagonal.Sigmas(vec(0.1, 0.2), True) #  0.1 rad std on bearing, 20cm on range
    
    # create the measurement values - indices are (pose id, landmark id)
    bearing11 = Rot2.fromDegrees(45)
    bearing21 = Rot2.fromDegrees(90)
    bearing32 = Rot2.fromDegrees(90)

def test_odometryExample(): 
    print("test_odmetryExample\n")
    print("=================================")

    # 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)
    prior_mean = Pose2(0, 0, 0)
    prior_noise  = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True)

    graph = NonlinearFactorGraph()
    graph.add(PriorFactorPose2(1, prior_mean, prior_noise))

    # Add odometry factors
    odometry = Pose2(2., 0., 0.)

    # For simplicity, we will use the same noise model for each odometry factor
    odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True)

    # Create odometry (Between) factors between consecutive poses
    graph.add(BetweenFactorPose2(1, 2, odometry, odometry_noise))
    graph.add(BetweenFactorPose2(2, 3, odometry, odometry_noise))

    block_str = '================'
    graph.printf(s="%s\nFactor Graph:\n" % block_str)

for i, posei in enumerate(poses):
        # generate the 2D measurement
        camera = SimpleCamera(posei, K)
        for j, pointj in enumerate(points):
            measurement = camera.project(pointj)
            print measurement

            sfactors[j].add_single(measurement, i, measurement_noise, K)

    # insert the smart factor in the graph
    for item in sfactors.itervalues(): 
        graph.add(item)

    # 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
    pose_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.3, 0.1, 0.1, 0.1))
    graph.add(PriorFactorPose3(0, poses[0], pose_noise))

    # Because the structure-from-motion problem has a scale ambiguity, the problem is
    # still under-constrained. Here we add a prior on the second pose x1, so this will
    # fix the scale by indicating the distance between x0 and x1.
    # Because these two are fixed, the rest of the poses will be also be fixed.
    graph.add(PriorFactorPose3(1, poses[1], pose_noise)) # add directly to graph

    graph.printf("Factor Graph:\n")

    # Create the initial estimate to the solution
    # Intentionally initialize the variables off from the ground truth
    initialEstimate = Values()
    delta = Pose3(Rot3.rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))

    for i, posei in enumerate(poses):

print("test_odmetryExample\n")
    print("=================================")

    # 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)
    prior_mean = Pose2(0, 0, 0)
    prior_noise  = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True)

    graph = NonlinearFactorGraph()
    graph.add(PriorFactorPose2(1, prior_mean, prior_noise))

    # Add odometry factors
    odometry = Pose2(2., 0., 0.)

    # For simplicity, we will use the same noise model for each odometry factor
    odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True)

    # Create odometry (Between) factors between consecutive poses
    graph.add(BetweenFactorPose2(1, 2, odometry, odometry_noise))
    graph.add(BetweenFactorPose2(2, 3, odometry, odometry_noise))

    block_str = '================'
    graph.printf(s="%s\nFactor Graph:\n" % block_str)

    # Create the data structure to hold the initialEstimate estimate to the
    # solution For illustrative purposes, these have been deliberately set to
    # incorrect values
    initial = Values()
    initial.insert(1, Pose2(0.5, 0.0, 0.2))
    initial.insert(2, Pose2(2.3, 0.1, -0.2))
    initial.insert(3, Pose2(4.1, 0.1, 0.1))

#  - x1 is fixed with a constraint, x2 is initialized with noisy values
    #  - No noise on measurements
    x1, x2 = symbol('x', 1), symbol('x', 2)
    l1, l2, l3 = symbol('l', 1), symbol('l', 2), symbol('l', 3)

    # Create a factor graph
    graph = NonlinearFactorGraph()

    # add a constraint on the starting pose
    first_pose = Pose3()
    graph.add(NonlinearEqualityPose3(x1, first_pose))

    # Create realistic calibration and measurement noise model
    # format: fx fy skew cx cy baseline
    K = Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
    stereo_model = Diagonal.Sigmas(vec(1.0, 1.0, 1.0))

    ## Add measurements
    # pose 1
    graph.add(GenericStereoFactor3D(
        StereoPoint2(520, 480, 440), stereo_model, x1, l1, K))
    graph.add(GenericStereoFactor3D(
        StereoPoint2(120,  80, 440), stereo_model, x1, l2, K))
    graph.add(GenericStereoFactor3D(
        StereoPoint2(320, 280, 140), stereo_model, x1, l3, K))

    # pose 2
    graph.add(GenericStereoFactor3D(
        StereoPoint2(570, 520, 490), stereo_model, x2, l1, K))
    graph.add(GenericStereoFactor3D(
        StereoPoint2( 70,  20, 490), stereo_model, x2, l2, K))
    graph.add(GenericStereoFactor3D(

# Add a prior on pose x1 at the origin. A prior factor consists of a mean
    # and a noise model (covariance matrix)
    prior = Pose2(0.0, 0.0, 0.0) #  prior mean is at origin
    prior_noise = Diagonal.Sigmas(vec(0.3, 0.3, 0.1), True) #  30cm std on x,y, 0.1 rad on theta
    graph.add(PriorFactorPose2(x1, prior, prior_noise)) #  add directly to graph
    
    # Add two odometry factors
    odometry = Pose2(2.0, 0.0, 0.0) #  create a measurement for both factors (the same in this case)
    odometry_noise = Diagonal.Sigmas(vec(0.2, 0.2, 0.1), True) #  20cm std on x,y, 0.1 rad on theta
    graph.add(BetweenFactorPose2(x1, x2, odometry, odometry_noise))
    graph.add(BetweenFactorPose2(x2, x3, odometry, odometry_noise))

    # Add Range-Bearing measurements to two different landmarks
    # create a noise model for the landmark measurements
    measurement_noise = Diagonal.Sigmas(vec(0.1, 0.2), True) #  0.1 rad std on bearing, 20cm on range
    
    # create the measurement values - indices are (pose id, landmark id)
    bearing11 = Rot2.fromDegrees(45)
    bearing21 = Rot2.fromDegrees(90)
    bearing32 = Rot2.fromDegrees(90)
    range11 = np.sqrt(4.0+4.0)
    range21 = 2.0
    range32 = 2.0
    
    # Add Bearing-Range factors
    graph.add(
        BearingRangeFactorPose2Point2(x1, l1, 
                                      bearing11, range11, measurement_noise))
    graph.add(
        BearingRangeFactorPose2Point2(x2, l1, 
                                      bearing21, range21, measurement_noise))