Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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(
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)
X = lambda i: int(gtsam.symbol(ord('x'), i))
def V(key):
"""Create symbol for velocity key."""
return gtsam.symbol(ord('v'), key)
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
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(