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