def __init__(self, twist=None, bias=None, dt=1e-2):
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
# setup interactive plotting
plt.ion()
# Setup loop as default scenario
if twist is not None:
(W, V) = twist
else:
# default = loop with forward velocity 2m/s, while pitching up
# with angular velocity 30 degree/sec (negative in FLU)
W = np.array([0, -math.radians(30), 0])
V = np.array([2, 0, 0])
self.scenario = gtsam.ConstantTwistScenario(W, V)
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)
def __init__(self,
twist: Optional[np.ndarray] = None,
bias: Optional[gtsam.imuBias.ConstantBias] = None,
params: Optional[gtsam.PreintegrationParams] = None,
dt: float = 1e-2):
"""Initialize with given twist, a pair(angularVelocityVector, velocityVector)."""
# setup interactive plotting
plt.ion()
# Setup loop as default scenario
if twist is not None:
(W, V) = twist
else:
# default = loop with forward velocity 2m/s, while pitching up
# with angular velocity 30 degree/sec (negative in FLU)
W = np.array([0, -np.radians(30), 0])
V = np.array([2, 0, 0])
self.scenario = gtsam.ConstantTwistScenario(W, V)
self.dt = dt
self.maxDim = 5
self.labels = list('xyz')
self.colors = list('rgb')
if params:
self.params = params
else:
# Default params with simple gravity constant
self.params = self.defaultParams(g=GRAVITY)
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)
# Create runner
self.runner = gtsam.ScenarioRunner(self.scenario, self.params, self.dt,
self.actualBias)
fig, self.axes = plt.subplots(4, 3)
fig.set_tight_layout(True)
def test_loop_runner(self):
# Forward velocity 2m/s
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
v = 2
w = math.radians(6)
W = np.array([0, -w, 0])
V = np.array([v, 0, 0])
scenario = gtsam.ConstantTwistScenario(W, V)
dt = 0.1
params = gtsam.PreintegrationParams.MakeSharedU(self.g)
bias = gtsam.imuBias.ConstantBias()
runner = gtsam.ScenarioRunner(scenario, params, dt, bias)
# Test specific force at time 0: a is pointing up
t = 0.0
a = w * v
np.testing.assert_almost_equal(np.array([0, 0, a + self.g]),
runner.actualSpecificForce(t))
def test_loop(self):
# Forward velocity 2m/s
# Pitch up with angular velocity 6 degree/sec (negative in FLU)
v = 2
w = math.radians(6)
W = np.array([0, -w, 0])
V = np.array([v, 0, 0])
scenario = gtsam.ConstantTwistScenario(W, V)
T = 30
np.testing.assert_almost_equal(W, scenario.omega_b(T))
np.testing.assert_almost_equal(V, scenario.velocity_b(T))
np.testing.assert_almost_equal(np.cross(W, V),
scenario.acceleration_b(T))
# R = v/w, so test if loop crests at 2*R
R = v / w
T30 = scenario.pose(T)
np.testing.assert_almost_equal(np.array([-math.pi, 0, -math.pi]),
T30.rotation().xyz())
self.assert_(gtsam.Point3(0, 0, 2 * R).equals(T30.translation()))
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 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
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
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
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()
# Incremental solution
isam.update(newgraph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()