Coordinate Systems

Coordinate frame definitions for the AV2.

Overview 

The AV2 uses several coordinate systems. Understanding these frames and their transformations is essential for correct operation.

Frame Summary 

Frame	X-axis	Y-axis	Z-axis
Vehicle	Forward	Left	Up
World (UTM)	East	North	Up
LIDAR	Forward	Left	Up
Camera	Right	Down	Forward
Grid	Forward	Left	N/A (2D)

Vehicle Frame 

The primary reference frame for the vehicle, following ISO 8855 convention.

Definition 

Origin: Center of rear axle, at ground level
X-axis: Points forward (positive ahead)
Y-axis: Points left (positive to left)
Z-axis: Points up (positive upward)

                  Z (up)
                   │
                   │
                   │
         Y (left)  │
           ◄───────┼───────► -Y (right)
                   │
                   │
                   │
                   ▼ -Z (down)

              (top view)

         Y (left)
           │
           │
           ▼
 ─────────────────────
│                     │
│                     │
│         X           │──────► X (forward)
│      (origin)       │
│         ●           │
 ─────────────────────

Usage 

Sensor mounting positions
Point cloud output (after transformation)
Control commands (steering angle, velocity)

World Frame (UTM)

The global reference frame for navigation.

Definition 

Origin: UTM zone reference point
X-axis: East (positive eastward)
Y-axis: North (positive northward)
Z-axis: Up (positive upward)

This is an East-North-Up (ENU) coordinate system.

UTM Projection 

GPS coordinates (WGS84 latitude/longitude) are converted to UTM meters:

from pyproj import Transformer

# WGS84 to UTM Zone 11N (Southern California)
transformer = Transformer.from_crs("EPSG:4326", "EPSG:32611")

lat, lon = 34.0577, -117.8215  # Cal Poly Pomona
east, north = transformer.transform(lat, lon)
# Result: east=427859.2, north=3769432.1 meters

Vehicle Heading 

Heading (yaw) is measured from North, clockwise positive:

0° = Facing North
90° = Facing East
180° = Facing South
270° = Facing West

          North (0°)
             │
             │
West (270°)──┼──► East (90°)
             │
             │
          South (180°)

LIDAR Frame 

The Velodyne VLP-16 sensor frame.

Definition 

Origin: Center of LIDAR sensor
X-axis: Forward (cable pointing backward)
Y-axis: Left
Z-axis: Up

Note

The LIDAR frame is aligned with the vehicle frame when properly mounted.

Transformation 

Points from LIDAR frame to vehicle frame:

# LIDAR mounted at (0.3, 0, 1.8) relative to vehicle origin
def lidar_to_vehicle(points_lidar):
    # points_lidar: Nx3 array [x, y, z]
    translation = np.array([0.3, 0.0, 1.8])
    points_vehicle = points_lidar + translation
    return points_vehicle

Camera Frame 

Standard computer vision (OpenCV) camera frame.

Definition 

Origin: Camera optical center
X-axis: Right (positive rightward in image)
Y-axis: Down (positive downward in image)
Z-axis: Forward (positive into scene)

Camera Frame:

    ──────────────────────
   │ ●───────► X (right)  │
   │ │                    │
   │ │                    │
   │ ▼ Y (down)           │
   │                      │
    ──────────────────────
         Image Plane

   Z points out of camera (into scene)

Transformation to Vehicle Frame 

# Camera frame to vehicle frame rotation
# Camera: X-right, Y-down, Z-forward
# Vehicle: X-forward, Y-left, Z-up

R_cam_to_vehicle = np.array([
    [0, 0, 1],   # Vehicle X = Camera Z
    [-1, 0, 0],  # Vehicle Y = -Camera X
    [0, -1, 0]   # Vehicle Z = -Camera Y
])

def camera_to_vehicle(points_camera, R, t):
    """Transform points from camera to vehicle frame."""
    points_vehicle = (R @ points_camera.T).T + t
    return points_vehicle

Occupancy Grid Frame 

The 2D grid used for obstacle mapping.

Definition 

Origin: Vehicle center (can be offset)
X-axis: Forward
Y-axis: Left
Resolution: Configurable (default 0.1 m/cell)

Grid Frame (top-down view):

      Y (columns, left)
      ▲
      │
      │
      │
      ├───────────────────►  X (rows, forward)
     (0,0)

Grid indices:
- Row 0 = rear of grid
- Column 0 = right side of grid

Grid to World Transformation 

def grid_to_world(grid_x, grid_y, vehicle_x, vehicle_y, vehicle_heading):
    """Convert grid coordinates to world coordinates."""
    # Grid to vehicle frame
    veh_x = (grid_x - grid_center_x) * resolution
    veh_y = (grid_y - grid_center_y) * resolution

    # Vehicle to world (rotate by heading)
    cos_h = np.cos(vehicle_heading)
    sin_h = np.sin(vehicle_heading)
    world_x = vehicle_x + veh_x * cos_h - veh_y * sin_h
    world_y = vehicle_y + veh_x * sin_h + veh_y * cos_h

    return world_x, world_y

Common Transformations 

GPS to Vehicle Relative 

Convert a GPS waypoint to vehicle-relative coordinates:

def gps_to_vehicle_relative(waypoint_lat, waypoint_lon,
                             vehicle_lat, vehicle_lon,
                             vehicle_heading):
    # Convert both to UTM
    wp_x, wp_y = gps_to_utm(waypoint_lat, waypoint_lon)
    veh_x, veh_y = gps_to_utm(vehicle_lat, vehicle_lon)

    # Relative position in world frame
    dx = wp_x - veh_x
    dy = wp_y - veh_y

    # Rotate to vehicle frame
    cos_h = np.cos(-vehicle_heading)
    sin_h = np.sin(-vehicle_heading)
    rel_x = dx * cos_h - dy * sin_h  # Forward
    rel_y = dx * sin_h + dy * cos_h  # Left

    return rel_x, rel_y

LIDAR Point to World 

Transform a LIDAR point to world coordinates:

def lidar_to_world(point_lidar, vehicle_pose):
    """
    point_lidar: [x, y, z] in LIDAR frame
    vehicle_pose: [x, y, heading] in world frame
    """
    # LIDAR to vehicle
    lidar_offset = [0.3, 0.0, 1.8]
    point_vehicle = point_lidar + lidar_offset

    # Vehicle to world
    veh_x, veh_y, heading = vehicle_pose
    cos_h = np.cos(heading)
    sin_h = np.sin(heading)

    world_x = veh_x + point_vehicle[0] * cos_h - point_vehicle[1] * sin_h
    world_y = veh_y + point_vehicle[0] * sin_h + point_vehicle[1] * cos_h
    world_z = point_vehicle[2]

    return [world_x, world_y, world_z]

Reference Standards 

ISO 8855: Road vehicles — Vehicle dynamics and road-holding ability — Vocabulary
REP 103: ROS Standard Units of Measure and Coordinate Conventions
UTM: Universal Transverse Mercator coordinate system