baglab.geometry¶
baglab.geometry
¶
Geometry utilities for ROS2 message data in DataFrames.
Provides conversions and computations commonly needed when working with
geometry_msgs types (Quaternion, Point, Pose, Twist, etc.) stored as
DataFrames or :class:~baglab.io.FieldGroup objects.
angle_diff(a, b)
¶
normalize_angle(angle)
¶
unwrap(angle)
¶
align_time(df1, df2, time_col=None)
¶
Align two DataFrames by time via linear interpolation.
The output DataFrames share the same time index (the union of both input indices, restricted to the overlapping time range). Numeric columns are interpolated linearly; non-numeric columns are forward-filled.
Parameters¶
df1, df2 : pd.DataFrame
DataFrames to align. If time_col is None, the existing index
(assumed to be a DatetimeIndex or numeric) is used. Otherwise the
named column is used as the time axis.
time_col : str | None
Column name to use as time. When provided, this column is set as
the index before interpolation.
Returns¶
tuple[pd.DataFrame, pd.DataFrame] Aligned copies of df1 and df2.
interp_pose(df, target_time, position_cols=('position.x', 'position.y'), orientation_cols=('orientation.x', 'orientation.y', 'orientation.z', 'orientation.w'))
¶
Interpolate pose data at target times.
Position is linearly interpolated. Orientation is interpolated via Spherical Linear Interpolation (SLERP) of quaternions.
Parameters¶
df : pd.DataFrame Must have a numeric index (time in seconds) and contain the position and orientation columns. target_time : array-like Times at which to interpolate. position_cols : tuple[str, str] Column names for position x and y. orientation_cols : tuple[str, str, str, str] Column names for quaternion x, y, z, w.
Returns¶
pd.DataFrame Interpolated pose with columns matching position_cols and orientation_cols.
match_by_time(t_query, t_source)
¶
Find the nearest source index for each query time.
Uses binary search (np.searchsorted) for O(N log M) performance.
Parameters¶
t_query : pd.Series | ndarray Query times (need not be sorted). t_source : pd.Series | ndarray Source times (must be monotonically increasing).
Returns¶
np.ndarray Integer indices into t_source, one per element in t_query.
resample(df, dt, time_col=None)
¶
Resample a DataFrame at uniform time intervals.
Parameters¶
df : pd.DataFrame
Input data.
dt : float
Desired time step in seconds.
time_col : str | None
Column name to use as time. If None, the index is used
(must be numeric, e.g. from :func:~baglab.stamp_to_sec).
Returns¶
pd.DataFrame Resampled data with uniform time index.
to_numpy_2d(fg)
¶
to_numpy_3d(fg)
¶
cumulative_distance(position)
¶
distance_2d(pos1, pos2)
¶
distance_3d(pos1, pos2)
¶
to_xy(position)
¶
to_xyz(position)
¶
pose_error(actual_pose, ref_pose, frame='xy')
¶
Compute pose error between actual and reference poses.
Returns along-track, cross-track, and yaw errors. The decomposition axes depend on frame.
Parameters¶
actual_pose, ref_pose : FieldGroup | DataFrame | dict | object Pose data. Accepted formats:
- DataFrame with ``position.x``, ``position.y``,
``orientation.x/y/z/w`` (Pose format).
- DataFrame with ``x``, ``y``, ``yaw`` columns (xyyaw format).
- dict or object with ``x``, ``y``, ``yaw`` keys/attributes
(scalar — returns dict).
frame : str Coordinate frame for along/cross decomposition:
- ``"xy"`` (default): world axes. along = dx, cross = dy.
- ``"ref_heading"``: reference yaw direction.
- ``"ref_path"``: tangent of the reference path (estimated from
consecutive positions).
Returns¶
pd.DataFrame | dict[str, float]
Columns/keys: along, cross, yaw.
pose_to_xyyaw(pose)
¶
Extract x, y, yaw from a Pose.
Parameters¶
pose : FieldGroup | DataFrame | object Accepted formats:
- DataFrame with ``position.x``, ``position.y``,
``orientation.x/y/z/w`` columns.
- FieldGroup pointing at a Pose level.
- ROS msg Pose object with ``position`` and ``orientation``
attributes (returns dict).
Returns¶
dict[str, float] | pd.DataFrame
{"x": …, "y": …, "yaw": …} for scalar input,
or DataFrame with columns x, y, yaw.
quat_to_rpy(orientation)
¶
Convert quaternion (x, y, z, w) to roll, pitch, yaw in radians.
Parameters¶
orientation : FieldGroup | DataFrame | object
Must contain columns/fields/attributes x, y, z, w.
Accepts a single ROS msg (returns dict) or a DataFrame/FieldGroup
(returns DataFrame).
Returns¶
dict[str, float] | pd.DataFrame
{"roll": …, "pitch": …, "yaw": …} for scalar input,
or DataFrame with columns roll, pitch, yaw.