# How to use the gtsam.imuBias_ConstantBias 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 / ImuFactorExample2.py View on Github
``````# 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))

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))``````
borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github
``````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))
elif i >= 2:  # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))

if i > 0:
if i % 5 == 0:
factor = gtsam.BetweenFactorConstantBias(

# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)

imufac = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)

# insert new velocity, which is wrong``````
borglab / gtsam / cython / gtsam / examples / PreintegrationExample.py View on Github
``````self.dt = dt

self.maxDim = 5
self.labels = list('xyz')
self.colors = list('rgb')

# Create runner
self.g = 10  # simple gravity constant
self.params = self.defaultParams(self.g)

if bias is not None:
self.actualBias = bias
else:
accBias = np.array([0, 0.1, 0])
gyroBias = np.array([0, 0, 0])
self.actualBias = gtsam.imuBias_ConstantBias(accBias, gyroBias)

self.runner = gtsam.ScenarioRunner(
self.scenario, self.params, self.dt, self.actualBias)``````
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
``````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))
elif i >= 2:  # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))

if i > 0:
if i % 5 == 0:
factor = gtsam.BetweenFactorConstantBias(

# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)

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)
accum.resetIntegration()``````
borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github
``````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))

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``````

## gtsam

Georgia Tech Smoothing And Mapping library

BSD-2-Clause