How to use the gtsam.symbol 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 spillai / pybot / deprecated / mapping / test_gtsam.py View on Github external
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
github spillai / pybot / deprecated / mapping / test_gtsam.py View on Github external
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
github spillai / pybot / deprecated / mapping / test_gtsam.py View on Github external
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(
github borglab / gtsam / cython / gtsam / examples / VisualISAMExample.py View on Github external
def symbol(name: str, index: int) -> int:
    """ helper for creating a symbol without explicitly casting 'name' from str to int """
    return gtsam.symbol(ord(name), index)
github borglab / gtsam / cython / gtsam / examples / ImuFactorExample.py View on Github external
def V(key):
    """Create symbol for velocity key."""
    return gtsam.symbol(ord('v'), key)
github borglab / gtsam / cython / gtsam / utils / visual_isam.py View on Github external
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)
        newFactors.add(
            gtsam.GenericProjectionFactorCal3_S2(
                zij, data.noiseModels.measurement,
                symbol(ord('x'), currPoseIndex), jj, data.K))
        # TODO: initialize with something other than truth
        if not result.exists(jj) and not initialEstimates.exists(jj):
            lmInit = truth.points[j]
            initialEstimates.insert(jj, lmInit)

    # Initial estimates for the new pose.
    prevPose = result.atPose3(symbol(ord('x'), prevPoseIndex))
    initialEstimates.insert(
        symbol(ord('x'), currPoseIndex), prevPose.compose(odometry))

    # Update ISAM
    # figure(1)tic
github borglab / gtsam / cython / gtsam / examples / PlanarSLAMExample.py View on Github external
from __future__ import print_function

import numpy as np

import gtsam

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

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

# Create the keys corresponding to unknown variables in the factor graph
X1 = gtsam.symbol(ord('x'), 1)
X2 = gtsam.symbol(ord('x'), 2)
X3 = gtsam.symbol(ord('x'), 3)
L1 = gtsam.symbol(ord('l'), 4)
L2 = gtsam.symbol(ord('l'), 5)

# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))

# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(gtsam.BetweenFactorPose2(
    X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
    X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))

# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(gtsam.BearingRangeFactor2D(