Scenario Class Hierarchy

Overview¶

The Scenario class hierarchy provides a simple way to define theoretical trajectories for testing navigation algorithms and factors, particularly IMU factors.

The base Scenario class is abstract and defines an interface that requires subclasses to provide the ground truth pose, angular velocity (in body frame), linear velocity (in nav frame), and linear acceleration (in nav frame) at any given continuous time t.

Concrete subclasses like ConstantTwistScenario and AcceleratingScenario implement specific types of motion.

Base Class: `Scenario`¶

Defines the required interface for any trajectory scenario.

Pure Virtual Methods (must be implemented by subclasses):

pose(t): Returns the Pose3 of the body in the navigation frame at time t.
omega_b(t): Returns the angular velocity Vector3 in the body frame at time t.
velocity_n(t): Returns the linear velocity Vector3 in the navigation frame at time t.
acceleration_n(t): Returns the linear acceleration Vector3 in the navigation frame at time t.

Derived Methods (provided by the base class):

rotation(t): Returns the Rot3 part of pose(t).
navState(t): Returns the NavState (Pose + Nav Velocity) at time t.
velocity_b(t): Calculates linear velocity in the body frame using $v_b = R_{bn} v_n$ .
acceleration_b(t): Calculates linear acceleration in the body frame using $a_b = R_{bn} a_n$ .

Concrete Scenarios¶

`ConstantTwistScenario`¶

Description: Models motion with a constant twist (angular velocity $\omega_b$ and linear velocity $v_b$ ) defined in the body frame.
Motion: Results in helical or circular motion trajectories.
Constructors:
- ConstantTwistScenario(omega_b, velocity_b, initial_nTb=Pose3())
- ConstantTwistScenario(twist_vector_6d, initial_nTb=Pose3())
Key Behavior: omega_b(t) is constant. velocity_n(t) and acceleration_n(t) are calculated based on the constant body-frame velocities and the changing orientation.

`AcceleratingScenario`¶

Description: Models motion with constant linear acceleration $a_n$ defined in the navigation frame and constant angular velocity $\omega_b$ defined in the body frame.
Motion: Represents scenarios like accelerating along a straight line while potentially rotating (e.g., aircraft takeoff, car acceleration).
Constructor: AcceleratingScenario(initial_nRb, initial_p_n, initial_v_n, const_a_n, const_omega_b=zero)
Key Behavior: acceleration_n(t) and omega_b(t) are constant. velocity_n(t) and pose(t) are integrated from the initial conditions and constant rates.

Usage Example¶

Instantiate a scenario and query its properties at different times.

import gtsam
import numpy as np

# --- ConstantTwistScenario Example: Moving in a Circle --- 
# Forward velocity 2 m/s, turning left (positive Z rot) at ~5.73 deg/s (0.1 rad/s)
omega_b_circle = np.array([0.0, 0.0, 0.1]) 
vel_b_circle = np.array([2.0, 0.0, 0.0]) # Forward velocity along body X
initial_pose_circle = gtsam.Pose3() # Start at origin

circle_scenario = gtsam.ConstantTwistScenario(omega_b_circle, vel_b_circle, initial_pose_circle)

print("--- Circle Scenario ---")
time_t = 1.0 # seconds
print(f"Pose at t={time_t}:")
circle_scenario.pose(time_t).print()
print(f"Nav Velocity at t={time_t}: {circle_scenario.velocity_n(time_t)}")
print(f"Body Omega at t={time_t}: {circle_scenario.omega_b(time_t)}")
print(f"Nav Acceleration at t={time_t}: {circle_scenario.acceleration_n(time_t)}") # Centripetal

# --- AcceleratingScenario Example: Accelerating Straight --- 
initial_pose_accel = gtsam.Pose3() 
initial_vel_n = np.array([0.0, 0.0, 0.0])
const_accel_n = np.array([0.5, 0.0, 0.0]) # Accelerate along nav X
const_omega_b = np.array([0.0, 0.0, 0.0]) # No rotation

accel_scenario = gtsam.AcceleratingScenario(
    initial_pose_accel.rotation(), initial_pose_accel.translation(),
    initial_vel_n, const_accel_n, const_omega_b
)

print("\n--- Accelerating Scenario ---")
time_t = 2.0 # seconds
print(f"Pose at t={time_t}:")
accel_scenario.pose(time_t).print()
print(f"Nav Velocity at t={time_t}: {accel_scenario.velocity_n(time_t)}")
print(f"Nav Acceleration at t={time_t}: {accel_scenario.acceleration_n(time_t)}")

--- Circle Scenario ---
Pose at t=1.0:
R: [
	0.995004, -0.0998334, 0;
	0.0998334, 0.995004, 0;
	0, 0, 1
]
t:   1.99667 0.0999167         0
Nav Velocity at t=1.0: [1.99000833 0.19966683 0.        ]
Body Omega at t=1.0: [0.  0.  0.1]
Nav Acceleration at t=1.0: [-0.01996668  0.19900083  0.        ]

--- Accelerating Scenario ---
Pose at t=2.0:
R: [
	1, 0, 0;
	0, 1, 0;
	0, 0, 1
]
t: 1 0 0
Nav Velocity at t=2.0: [1. 0. 0.]
Nav Acceleration at t=2.0: [0.5 0.  0. ]

These Scenario objects are primarily used as input to the ScenarioRunner class for generating simulated IMU data.

Source¶

Scenario.h