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.

github borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github external
# 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)
    biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
    biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
                                              biasnoise)
    newgraph.push_back(biasprior)
    initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
    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))
github borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github external
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:
            # Add Bias variables periodically
            if i % 5 == 0:
                biasKey += 1
                factor = gtsam.BetweenFactorConstantBias(
                    biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
                newgraph.add(factor)
                initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())

            # 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)

            # Add Imu Factor
            imufac = gtsam.ImuFactor(
                X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
            newgraph.add(imufac)

            # insert new velocity, which is wrong
github borglab / gtsam / cython / gtsam / examples / PreintegrationExample.py View on Github external
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)
github borglab / gtsam / cython / gtsam / examples / ImuFactorExample.py View on Github external
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 = (
            np.array([math.radians(30), -math.radians(30), 0]), self.velocity)

        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)
github borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github external
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:
            # Add Bias variables periodically
            if i % 5 == 0:
                biasKey += 1
                factor = gtsam.BetweenFactorConstantBias(
                    biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
                newgraph.add(factor)
                initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())

            # 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)

            # Add Imu Factor
            imufac = gtsam.ImuFactor(
                X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
            newgraph.add(imufac)

            # insert new velocity, which is wrong
            initialEstimate.insert(V(i), n_velocity)
            accum.resetIntegration()
github borglab / gtsam / cython / gtsam / examples / ImuFactorExample2.py View on Github external
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)
    biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
    biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
                                              biasnoise)
    newgraph.push_back(biasprior)
    initialEstimate.insert(biasKey, gtsam.imuBias_ConstantBias())
    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