def testOffsetTrajectories():
for i in range(10):
T = RandomTrajectory()
t = T.positionKeyFrames.timestamps
dt = t[1] - t[0]
p = T.position(t)
r = T.rotation(t)
offset = randomPosition()
rotationOffset = randomRotation()
oT = OffsetTrajectory(T, offset, rotationOffset)
offsetPositions = p + r.rotateVector(offset)
offsetRotations = r * rotationOffset
yield checkTrajectory, oT, TimeSeries(t,offsetPositions), TimeSeries(t, offsetRotations)
def testOffsetTrajectories():
for i in range(10):
T = RandomTrajectory()
t = T.positionKeyFrames.timestamps
dt = t[1] - t[0]
p = T.position(t)
r = T.rotation(t)
offset = randomPosition()
rotationOffset = randomRotation()
oT = OffsetTrajectory(T, offset, rotationOffset)
offsetPositions = p + r.rotateVector(offset)
offsetRotations = r * rotationOffset
yield checkTrajectory, oT, TimeSeries(t, offsetPositions), TimeSeries(
t, offsetRotations)
def testOrient3Model():
env = Environment()
calibrator = ScaleAndOffsetCalibrator(env, 1000, 1 / 100, 10)
for i in range(3):
imu = Orient3IMU()
cal = calibrator.calibrate(imu)
sim = Simulation(environment=env)
traj = RandomTrajectory()
imu.simulation = sim
imu.trajectory = traj
sim.time = imu.trajectory.startTime
BasicIMUBehaviour(imu, 1 / 256, calibration=cal, initialTime=sim.time)
sim.run(traj.endTime)
t = imu.accelerometer.calibratedMeasurements.timestamps
r = traj.rotation(t)
a = r.rotateFrame(
traj.acceleration(t) - env.gravitationalField(traj.position(t), t))
m = r.rotateFrame(env.magneticField(traj.position(t), t))
w = r.rotateFrame(traj.rotationalVelocity(t))
g = env.gravitationalField.nominalMagnitude
a_within_5g = abs(a) < 5 * g
for i in range(3):
yield assert_array_almost_equal, \
np.array([a[i ,a_within_5g[i]]]), \
np.array([imu.accelerometer.calibratedMeasurements(t)
[i, a_within_5g[i]]]), -2
yield assert_array_almost_equal, m, \
imu.magnetometer.calibratedMeasurements(t), 0
yield assert_array_almost_equal, w, \
imu.gyroscope.calibratedMeasurements(t), 0
def testOrient3Model():
env = Environment()
calibrator = ScaleAndOffsetCalibrator(env, 1000, 1/100, 10)
for i in range(3):
imu = Orient3IMU()
cal = calibrator.calibrate(imu)
sim = Simulation(environment=env)
traj = RandomTrajectory()
imu.simulation = sim
imu.trajectory = traj
sim.time = imu.trajectory.startTime
BasicIMUBehaviour(imu, 1/256, calibration=cal, initialTime=sim.time)
sim.run(traj.endTime)
t = imu.accelerometer.calibratedMeasurements.timestamps
r = traj.rotation(t)
a = r.rotateFrame(traj.acceleration(t) -
env.gravitationalField(traj.position(t), t))
m = r.rotateFrame(env.magneticField(traj.position(t), t))
w = r.rotateFrame(traj.rotationalVelocity(t))
g = env.gravitationalField.nominalMagnitude
a_within_5g = abs(a) < 5*g
for i in range(3):
yield assert_array_almost_equal, \
np.array([a[i ,a_within_5g[i]]]), \
np.array([imu.accelerometer.calibratedMeasurements(t)
[i, a_within_5g[i]]]), -2
yield assert_array_almost_equal, m, \
imu.magnetometer.calibratedMeasurements(t), 0
yield assert_array_almost_equal, w, \
imu.gyroscope.calibratedMeasurements(t), 0
from imusim.behaviours.imu import BasicIMUBehaviour from imusim.platforms.imus import IdealIMU from imusim.simulation.base import Simulation from imusim.testing.random_data import RandomTrajectory from rsimusim.camera import * from rsimusim_legacy.world import * # Create environment that contains landmarks num_landmarks = 50 world_points = np.random.uniform(-100, 100, size=(3, num_landmarks)) world = NonBlockableWorld(world_points) world_environment = WorldEnvironment(world) sim = Simulation(environment=world_environment) trajectory = RandomTrajectory() # Create camera platform camera_model = PinholeModel(np.eye(3), (1920, 1080), 1. / 35, 30.0) camera = CameraPlatform(camera_model, simulation=sim, trajectory=trajectory) # IMU platform imu = IdealIMU(simulation=sim, trajectory=trajectory) # Set up a behaviour that runs on the # simulated Camera behaviour_camera = DefaultCameraBehaviour(camera) # Set up a behaviour that runs on the # simulated IMU dt = 1. / 200
