How to use the gtsam.NonlinearFactorGraph 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_unstable / examples / View on Github external
def BatchFixedLagSmootherExample():
    Runs a batch fixed smoother on an agent with two odometry
    sensors that is simply moving to the

    # Define a batch fixed lag smoother, which uses
    # Levenberg-Marquardt to perform the nonlinear optimization
    lag = 2.0
    smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)

    # Create containers to store the factors and linearization points
    # that will be sent to the smoothers
    new_factors = gtsam.NonlinearFactorGraph()
    new_values = gtsam.Values()
    new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()

    # Create  a prior on the first pose, placing it at the origin
    prior_mean = gtsam.Pose2(0, 0, 0)
    prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
    X1 = 0
    new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
    new_values.insert(X1, prior_mean)
    new_timestamps.insert(_timestamp_key_value(X1, 0.0))

    delta_time = 0.25
    time = 0.25

    while time <= 3.0:
        previous_key = 1000 * (time - delta_time)
github PoseNet-Mobile-Robot / Mobile-Robotics / View on Github external
def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1):
        """ priorMean and priorCov should be in 1 dimensional array

        # init the graph
        self.graph = gtsam.NonlinearFactorGraph()

        # init the iSam2 solver
        parameters = gtsam.ISAM2Params()
        self.isam = gtsam.ISAM2(parameters)

        # init container for initial values
        self.initialValues = gtsam.Values()

        # setting the current position
        self.currentKey = 1

        # current estimate
        self.currentEst = False
        self.currentPose = [0,0,0]
github borglab / gtsam / cython / gtsam / examples / View on Github external
target = gtsam.Point3(0, 0, 0)
    position = gtsam.Point3(radius, 0, 0)
    camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
    pose_0 = camera.pose()

    # Create the set of ground-truth landmarks and poses
    angular_velocity = math.radians(180)  # rad/sec
    delta_t = 1.0/18  # makes for 10 degrees per step

    angular_velocity_vector = vector3(0, -angular_velocity, 0)
    linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
    scenario = gtsam.ConstantTwistScenario(
        angular_velocity_vector, linear_velocity_vector, pose_0)

    # Create a factor graph
    newgraph = gtsam.NonlinearFactorGraph()

    # Create (incremental) ISAM2 solver
    isam = gtsam.ISAM2()

    # Create the initial estimate to the solution
    # Intentionally initialize the variables off from the ground truth
    initialEstimate = gtsam.Values()

    # 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
    noise = gtsam.noiseModel_Diagonal.Sigmas(
        np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
    newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))

    # Add imu priors
    biasKey = gtsam.symbol(ord('b'), 0)
github borglab / gtsam / cython / gtsam / examples / View on Github external
from __future__ import print_function

import numpy as np

import gtsam

import matplotlib.pyplot as plt
import gtsam.utils.plot as gtsam_plot

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

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

# 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)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0)  # prior at origin
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))

# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
# For simplicity, we will use the same noise model for each odometry factor
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))

# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
github borglab / gtsam / cython / gtsam / utils / View on Github external
def step(data, isam, result, truth, currPoseIndex):
    Do one step isam update
    @param[in] data: measurement data (odometry and visual measurements and their noiseModels)
    @param[in/out] isam: current isam object, will be updated
    @param[in/out] result: current result object, will be updated
    @param[in] truth: ground truth data, used to initialize new variables
    @param[currPoseIndex]: index of the current pose
    # iSAM expects us to give it a new set of factors
    # along with initial estimates for any new variables introduced.
    newFactors = gtsam.NonlinearFactorGraph()
    initialEstimates = gtsam.Values()

    # Add odometry
    prevPoseIndex = currPoseIndex - 1
    odometry = data.odometry[prevPoseIndex]
            symbol(ord('x'), prevPoseIndex),
            symbol(ord('x'), currPoseIndex), 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)
github borglab / gtsam / cython / gtsam / examples / View on Github external
# Add Imu Factor
            imufac = gtsam.ImuFactor(
                X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)

            # insert new velocity, which is wrong
            initialEstimate.insert(V(i), n_velocity)

        # Incremental solution
        isam.update(newgraph, initialEstimate)
        result = isam.calculateEstimate()

        # reset
        newgraph = gtsam.NonlinearFactorGraph()
github borglab / gtsam / cython / gtsam / examples / View on Github external
prior.print_('goal angle')
    model = gtsam.noiseModel_Isotropic.Sigma(dim=1, sigma=np.deg2rad(1))
    key = gtsam.symbol(ord('x'), 1)
    factor = gtsam.PriorFactorRot2(key, prior, model)

    Step 2: Create a graph container and add the factor to it

    Before optimizing, all factors need to be added to a Graph container,
    which provides the necessary top-level functionality for defining a
    system of constraints.

    In this case, there is only one factor, but in a practical scenario,
    many more factors would be added.
    graph = gtsam.NonlinearFactorGraph()
    graph.print_('full graph')

    Step 3: Create an initial estimate

    An initial estimate of the solution for the system is necessary to
    start optimization.  This system state is the "Values" instance,
    which is similar in structure to a dictionary, in that it maps
    keys (the label created in step 1) to specific values.

    The initial estimate provided to optimization will be used as
    a linearization point for optimization, so it is important that
    all of the variables in the graph have a corresponding value in
    this structure.