Skip to content

Geometry and types

SO(3)/SE(3) utilities.

Conventions used consistently across this package:

  • A pose T_wb maps a point from body frame b to world frame w: p_w = R_wb @ p_b + t_wb.
  • Quaternions are stored (w, x, y, z), the TUM trajectory file order. Dataset readers that supply a different order convert on read.
  • Hat/vee matrices map a 3-vector to/from a skew-symmetric matrix.
  • Rotation Jacobians and logs follow the standard robotics literature (Barfoot, Sola, Forster). Exp/log are the right Jacobians' inverses where a closed form exists; the branch is chosen so Exp(Log(R)) == R for all rotations without a sign flip at pi.

Nothing here is copied from an existing SLAM library. The formulas are the standard textbook ones and each is unit tested against an independent construction in tests/test_geometry.py.

Return the 3x3 skew-symmetric matrix of v.

Inverse of :func:skew.

Exponential map so(3) -> SO(3) (Rodrigues' formula).

Logarithmic map SO(3) -> so(3).

The rotation angle is recovered with

``theta = atan2(0.5 * ||vee(R - R^T)||, 0.5 * (trace(R) - 1))``

rather than arccos(0.5 * (trace(R) - 1)). This matters a great deal here: the increments this library has to resolve are small (a 200 Hz gyroscope sees 5 ms of rotation, a 20 Hz camera sees 50 ms), and arccos loses roughly half the significant digits as its argument approaches 1. With the arccos form the relative error in the recovered angle is of order 1/theta, so a 1 mrad increment is already 30% wrong -- which silently corrupts every gyro sample and every visual rotation residual. atan2 is well conditioned across the whole range.

Left Jacobian of SO(3).

Right Jacobian of SO(3).

The right Jacobian satisfies Exp(phi + d) ~= Exp(phi) Exp(J_r(phi) d) for small d, so it is the correct linearisation whenever a rotation error is expressed on the right of an existing rotation. It is the transpose of the left Jacobian: J_r(phi) == J_l(phi).T, and also J_r(phi) == J_l(-phi).

Vectorised :func:rot_log over a stack of rotations.

Accepts (3, 3) or (N, 3, 3) and returns (3,) or (N, 3).

Geodesic angle in radians of a stack of rotations.

Convert a (w, x, y, z) quaternion to a rotation matrix.

Convert a rotation matrix to a (w, x, y, z) quaternion.

Hamilton product a * b for (w, x, y, z) quaternions.

Geodesic angle in radians of the rotation represented by q.

Rotation angle in radians between two quaternions.

Exponential map se(3) -> SE(3).

xi = [rho (3), phi (3)] with translation in the tangent frame: T = Exp([phi], [J_l(phi) rho]).

Logarithmic map SE(3) -> se(3), inverse of :func:pose_exp.

Assemble a 4x4 pose from a rotation and a position.

Apply a 4x4 transform to an (N, 3) array of points.

Trajectory alignment (Umeyama) for consistent error reporting.

Two alignments are supported and the difference matters when reading results:

rigid Rigid transform, scale fixed to 1. Use this when the scale of the estimate is physically meaningful (metric odometry, GNSS-aided fusion). Any scale error shows up in the reported error instead of being absorbed.

sim3 Rigid transform plus a global scale. Use this to separate shape error from scale error, or to compare monocular estimates that have an arbitrary gauge.

Both follow Umeyama (1991) and use the same SVD-based construction.

A similarity transform mapping an estimate onto a reference.

4x4 matrix. The scale is kept out of the matrix on purpose.

Least-squares similarity transform target ~= scale * source @ R.T + t.

kind selects the model: "rigid" ignores with_scale and pins scale to 1.0.

Fit the alignment on a leading fraction of the trajectory.

The TUM VI benchmark reports both an "ate_start" (fit on the first 20% and evaluate everywhere) and a full "ate" number. Fitting only on the start is the stricter, more informative choice for drift analysis: it cannot hide accumulated drift inside the alignment itself. The returned mask marks the poses used for fitting.

Core data structures shared by the replay pipeline.

All time is in seconds (float64) relative to an arbitrary but recorded epoch. All poses are 4x4 T_wb matrices: body frame to world frame.

These containers are plain dataclasses holding numpy arrays. They are intentionally mutable-ish and cheap: a 141 s sequence at 120 Hz is ~17k poses and a few megabytes, so there is no reason to add a storage abstraction.

A timestamped sequence of poses.

(N, 4) as (w, x, y, z) -- this library's internal order.

Note that this is not the order used in TUM data.txt files, which are (qx, qy, qz, qw). The readers and writers in :mod:navkit.io.trajectory are the only place that conversion happens.

Return a copy with every timestamp shifted by dt seconds.

Return a copy with every pose left-multiplied by T.

Central-difference velocity in the world frame, (N, 3).

Body-frame angular rate, (N, 3), from central differences of R.

Inertial measurement, body frame.

Return a copy with every timestamp shifted by dt seconds.

A GNSS position fix, world frame, with a diagonal covariance.

Return a copy with unavailable fixes removed.

Contiguous [start, end] intervals where the fix is unavailable.

A sample at time t_i is treated as covering [t_i - dt/2, t_i + dt/2], so an outage is measured in wall-clock time rather than in sample counts.

A relative-pose measurement as a visual front end would emit it.

For the current frame i the measurement is the transform T_prev_cur = T_{i-1}^{-1} T_i expressed in the previous camera frame:

  • R_rel[i] = R_{i-1}^T R_i -- current-frame axes in the previous frame;
  • t_rel[i] = R_{i-1}^T (p_i - p_{i-1}) -- current origin in the previous frame.

R_rel is the rotation of T_prev_cur. It must not be confused with R_i R_{i-1}^T, the rotation of the inverse transform, because pairing that with t_rel yields a measurement that does not telescope. This is the output of a standard two-frame relative-pose estimator. The proxy generator documents the simplification of assuming a known, small camera-to-IMU extrinsic.

Return only the measurements that were not dropped.

Convert a (w, x, y, z) quaternion to a rotation matrix.

Vectorised :func:rot_log over a stack of rotations.

Accepts (3, 3) or (N, 3, 3) and returns (3,) or (N, 3).

Return the 3x3 skew-symmetric matrix of v.

Apply a 4x4 transform to an (N, 3) array of points.

Non-uniform central differences, with one-sided ends.

Deliberately simple: this repository replays trajectories at 100-200 Hz where a higher-order stencil buys nothing. second=True differentiates twice, which is what the IMU synthesis needs; its accuracy is limited by the ground-truth sample rate and is documented as such in docs/limitations.md.

Sample a trajectory at arbitrary times by SLERP + linear translation.

Queries outside the trajectory span are clamped to the endpoints and the out-of-range mask is returned, because silently extrapolating a pose is a reliable way to manufacture a fake metric.

Per-pose position error (m) and rotation error (rad) versus reference.

The estimate is associated with the reference by timestamp: the reference is interpolated at the estimate's timestamps. No alignment is applied here; alignment happens in the metric layer.