Cone Ordering

Cone ordering is the process of organizing scattered cone observations into sorted left and right track boundaries. This is a critical preprocessing step that transforms unordered cone detections into a structured representation suitable for trajectory optimization.

Problem Statement

Input: Unordered cone positions with color labels (blue = left, yellow = right)

Output: Two sorted lists of cones representing the left and right track boundaries, ordered by distance along the track

Challenge: Cones may be observed in any order, and observations may have:

Missing cones (occlusions, limited sensor range)
Noisy positions (sensor measurement errors)
Ambiguous pairings (wide track sections, complex geometries)

Distance-Based Ordering

The current implementation uses a greedy nearest-neighbor algorithm:

Algorithm Description

def distance_cone_order(cones, vehicle_state):
    1. Filter out cones behind the vehicle
    2. Start from vehicle position
    3. Repeat until all cones processed:
        a. Find nearest cone to current position
        b. Add cone to ordered list
        c. Move current position to that cone
    4. Interpolate to get N evenly-spaced points
    5. Return ordered left and right boundaries

Step 1: Filter Cones Behind Vehicle

Transform cones to vehicle frame and reject those with negative x-coordinates:

# Rotation matrix to vehicle frame
R = [[cos(-heading), -sin(-heading)],
     [sin(-heading),  cos(-heading)]]

cone_vehicle_frame = R @ (cone_position - vehicle_position)

if cone_vehicle_frame[0] > 0:  # Forward of vehicle
    keep_cone()

This prevents the algorithm from ordering cones in the wrong direction.

Step 2: Greedy Nearest Neighbor

Starting from the vehicle position, iteratively select the closest unprocessed cone:

current_position = vehicle_position
ordered_cones = []

while unprocessed_cones:
    distances = [||cone - current_position|| for cone in unprocessed_cones]
    nearest_idx = argmin(distances)
    nearest_cone = unprocessed_cones[nearest_idx]

    ordered_cones.append(nearest_cone)
    unprocessed_cones.remove(nearest_cone)
    current_position = nearest_cone

This produces a path that follows the cone boundary.

Step 3: Interpolation

The ordered cones are not evenly spaced, but the trajectory optimizer expects N uniformly distributed points. Linear interpolation creates this uniform distribution:

# Compute arc length parameterization
t = [0]
for i in range(1, len(ordered_cones)):
    dx = ordered_cones[i][0] - ordered_cones[i-1][0]
    dy = ordered_cones[i][1] - ordered_cones[i-1][1]
    t.append(t[-1] + sqrt(dx² + dy²))

# Interpolate at uniform spacing
t_uniform = linspace(0, t[-1], N)
interpolated_cones = interp1d(t, ordered_cones)(t_uniform)

This ensures the optimization has evenly distributed boundary points.

See: local_path/local_path/distance_cone_order.py:9-102

Implementation Details

Filtering

The filtering step is critical for correct operation:

heading = state[2]  # Vehicle heading angle
rotation_matrix = rotation(-heading)

for cone in cones:
    cone_rel = cone - vehicle_position
    cone_in_car_frame = rotation_matrix @ cone_rel
    if cone_in_car_frame[0] > 0:  # x > 0 means forward
        filtered_cones.append(cone)

Without this filter, the algorithm might order cones backwards or create loops.

Separate Left/Right Processing

Left and right boundaries are ordered independently:

Blue cones (left boundary) processed separately from yellow cones (right)
Each boundary ordered using the same greedy algorithm
Both start from the vehicle position

This prevents incorrect pairing between left and right sides.

Minimum Cone Requirements

The algorithm requires at least 2 cones per side:

if len(left_sorted) < 2 or len(right_sorted) < 2:
    return None, None  # Insufficient cones

With only 1 cone per side, interpolation is not possible.

Algorithm Characteristics

This is a simple greedy nearest-neighbor implementation that provides fast computation (~5-20ms) and is easy to understand. The algorithm works well for the majority of typical race track scenarios, handling straight sections and moderate curves effectively.

Known Limitations: We acknowledge that this distance-based approach does not handle all edge cases perfectly. The greedy nature of the algorithm can fail on complex track geometries such as sharp hairpins, very wide sections, or ambiguous cone pairings. We are currently working on developing a more robust cone ordering solution.

For the 2025 competition, this distance-based algorithm was selected as the primary approach due to its simplicity and consistent performance on typical tracks, despite its known limitations.

Edge Case Failures

Greedy Approach

The nearest-neighbor strategy can fail when:

Track boundaries cross or converge (e.g., sharp hairpins)
Wide track sections with ambiguous nearest cone
Cones form complex patterns not captured by simple distance

Example failure case:

Left boundary:    A -------- B
                             |
Right boundary:  C           D

If starting at A, algorithm might go A → C (nearest)
instead of A → B (correct track direction)

No Global Optimization

The algorithm processes cones sequentially without considering the entire set:

Cannot backtrack if early decisions were suboptimal
May create zigzag patterns in ambiguous situations
Doesn’t enforce smoothness or consistency

Edge Case Failures

Known problematic scenarios:

Very wide sections: Distance ambiguity between left/right sides
Hairpin turns: Cones may be geometrically close but far along track
Missing cones: Gaps in observations can break the ordering
Dense cone placement: Multiple cones at similar distances

Alternative Algorithm

An alternative cone ordering algorithm (funny_cone_ordering) is under development to address these limitations. It uses more sophisticated logic for pairing and ordering cones, but has not been fully validated on all edge cases.

Note: The system is configured to use distance-based ordering (distance_cone_order) as the primary algorithm. While the distance-based approach has known limitations, it provides consistent performance on typical race tracks. We acknowledge that neither the current distance-based algorithm nor the alternative algorithm handles all edge cases perfectly.

Visualization

For debugging cone ordering, the local planner publishes visualization markers:

Topic: /path/local/vizconeorder

Type: visualization_msgs/Marker (LINE_LIST)

Content: Red lines connecting paired left-right boundary points

This allows visual inspection of the cone ordering result in RViz:

# Each pair of left-right cones shown as a line
points = []
for left_cone, right_cone in zip(ordered_left, ordered_right):
    points.append(Point(x=left_cone[0], y=left_cone[1]))
    points.append(Point(x=right_cone[0], y=right_cone[1]))

If the lines are smooth and roughly perpendicular to the track, ordering is good. If lines cross or zigzag, ordering has failed.

Configuration

From all_settings.py:

class LocalOptSettings(metaclass=Settings):
    N: int = 10  # Number of interpolated points
    distance_cone_order: bool = True  # Use distance-based ordering

To disable distance-based ordering and use the alternative algorithm:

distance_cone_order: bool = False

However, this is not recommended for production use until the alternative is fully validated.