# How to use the gtsam.noiseModel_Isotropic 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.

borglab / gtsam / cython / gtsam / examples / VisualISAM2Example.py View on Github
``````# Intentionally initialize the variables off from the ground truth
initial_estimate.insert(X(i), pose.compose(gtsam.Pose3(
gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25), gtsam.Point3(0.05, -0.10, 0.20))))

# If this is the first iteration, add a prior on the first pose to set the
# coordinate frame and a prior on the first landmark to set the scale.
# Also, as iSAM solves incrementally, we must wait until each is observed
# at least twice before adding it to iSAM.
if i == 0:
# Add a prior on pose x0
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array(
[0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))  # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))

# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
graph.push_back(gtsam.PriorFactorPoint3(
L(0), points[0], point_noise))  # add directly to graph

# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initial_estimate.insert(L(j), gtsam.Point3(
point.x()-0.25, point.y()+0.20, point.z()+0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
# If accuracy is desired at the expense of time, update(*) can be called additional
# times to perform multiple optimizer iterations every step.
isam.update()
current_estimate = isam.calculateEstimate()``````
borglab / gtsam / cython / gtsam / examples / SimpleRotation.py View on Github
``````The "prior" in this case is the measurement from a sensor,
with a model of the noise on the measurement.

The "Key" created here is a label used to associate parts of the
state (stored in "RotValues") with particular factors.  They require
an index to allow for lookup, and should be unique.

In general, creating a factor requires:
- A key or set of keys labeling the variables that are acted upon
- A measurement value
- A measurement model with the correct dimensionality for the factor
"""
prior.print_('goal angle')
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.push_back(factor)
graph.print_('full graph')``````
borglab / gtsam / cython / gtsam / examples / VisualISAMExample.py View on Github
``````# Add an initial guess for the current pose
initial_estimate.insert(symbol('x', i), initial_xi)

# If this is the first iteration, add a prior on the first pose to set the coordinate frame
# and a prior on the first landmark to set the scale
# Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
if i == 0:
# Add a prior on pose x0, with 0.3 rad std on roll,pitch,yaw and 0.1m 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)

# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(symbol('l', 0), points[0], point_noise)
graph.push_back(factor)

# Add initial guesses to all observed landmarks
noise = np.array([-0.25, 0.20, 0.15])
for j, point in enumerate(points):
# Intentionally initialize the variables off from the ground truth
initial_lj = points[j].vector() + noise
initial_estimate.insert(symbol('l', j), Point3(initial_lj))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
current_estimate = isam.estimate()
print('*' * 50)
print('Frame {}:'.format(i))
current_estimate.print_('Current estimate: ')``````
borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github
``````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))

biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasnoise)
newgraph.push_back(biasprior)
velnoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)

# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)

accum = gtsam.PreintegratedImuMeasurements(PARAMS)

# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t  # simulation time
if i == 0:  # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))``````
borglab / gtsam / cython / gtsam / utils / visual_data_generator.py View on Github
``````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)``````
borglab / gtsam / cython / gtsam / examples / ImuFactorExample.py View on Github
``````def __init__(self):
self.velocity = np.array([2, 0, 0])
self.priorNoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
self.velNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)

# Choose one of these twists to change scenario:
zero_twist = (np.zeros(3), np.zeros(3))
forward_twist = (np.zeros(3), self.velocity)
loop_twist = (np.array([0, -math.radians(30), 0]), self.velocity)
sick_twist = (

accBias = np.array([-0.3, 0.1, 0.2])
gyroBias = np.array([0.1, 0.3, -0.1])
bias = gtsam.imuBias_ConstantBias(accBias, gyroBias)

dt = 1e-2
super(ImuFactorExample, self).__init__(sick_twist, bias, dt)``````
borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github
``````plt.pause(1)

# IMU preintegration parameters
# Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
g = 9.81
n_gravity = vector3(0, 0, -g)
PARAMS = gtsam.PreintegrationParams.MakeSharedU(g)
I = np.eye(3)
PARAMS.setAccelerometerCovariance(I * 0.1)
PARAMS.setGyroscopeCovariance(I * 0.1)
PARAMS.setIntegrationCovariance(I * 0.1)
PARAMS.setUse2ndOrderCoriolis(False)
PARAMS.setOmegaCoriolis(vector3(0, 0, 0))

BIAS_COVARIANCE = gtsam.noiseModel_Isotropic.Variance(6, 0.1)
DELTA = gtsam.Pose3(gtsam.Rot3.Rodrigues(0, 0, 0),
gtsam.Point3(0.05, -0.10, 0.20))

def IMU_example():
"""Run iSAM 2 example with IMU factor."""

up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 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``````

## gtsam

Georgia Tech Smoothing And Mapping library

BSD-2-Clause