# How to use the gtsam.noiseModel_Diagonal.Sigmas 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 / Pose2SLAMExample.py View on Github
``````import numpy as np

import gtsam

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

def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)

# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))

# 1. Create a factor graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()

# 2a. Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)

# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))``````
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 / Pose2SLAMExample.py View on Github
``````import numpy as np

import gtsam

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

def vector3(x, y, z):
"""Create 3d double numpy array."""
return np.array([x, y, z], dtype=np.float)

# Create noise models
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.3, 0.3, 0.1))
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(vector3(0.2, 0.2, 0.1))

# 1. Create a factor graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()

# 2a. Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix)

# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))
3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))