Driving a track the car has never seen before
Formula Student Driverless tracks are marked out with colored cones, blue on the left and yellow on the right, and the car gets no map of the layout beforehand. I built this perception and control stack in two stages. First a LiDAR-only reactive controller that completes laps but oscillates badly, then a fused color-aware version that fixed the oscillation by finally knowing which side was which.
Fast clustering, and a controller that cannot tell left from right
Raw LiDAR returns come in as a flat array of points in the sensor frame. Sorted by the rotating scanner's angular order, consecutive points sitting within 0.1 m of each other get grouped into the same cluster. That threshold is calibrated to the roughly 30 cm base of a regulation FSAE cone. The scan ordering is what keeps the whole thing O(n), with no DBSCAN and no k-nearest-neighbor search, just a single pass that exploits the geometry the sensor already gives you.
Steering off that alone is a binary decision. More cones on the left than the right means steer right, and the other way around. On a curve it works fine, because the inside boundary genuinely is denser. On a straight the cone distribution is roughly symmetric, so the comparison flips sign almost at random from one frame to the next and the car weaves down the track. Fixing that weave is the reason the rest of this project exists.
YOLO knows what color a cone is, until the simulator renders it
A YOLOv5 detector pretrained on the FSOCO dataset classifies each cone by color. FSOCO is real-world photography though, and the simulator's rendered cones confused it constantly, with blue cones coming back labeled yellow or orange all the time at medium range. What fixed it was a verification cascade layered on top of the raw detection. Stripe brightness runs first, since a bright stripe means blue and a dark one means probably yellow. When brightness lands in the ambiguous middle, a BGR channel-dominance check takes over. HSV hue analysis is the last resort when the other two cannot decide.
That 70 percent ceiling is physical, not something better code fixes. Past 5 m a cone covers roughly 8 by 10 pixels in a 640 by 480 frame, background asphalt dominates the bounding box, and the BGR channels converge to within about 24 units of gray. There is no color signal left to recover at that point. What saves you is that cones get bigger as you approach them, well before they matter for steering.
Depth from the LiDAR, color from the camera
A standard pinhole model unprojects the bottom-center pixel of each detection into a camera ray, then intersects that ray with the ground plane to get a metric position in the vehicle frame. Camera and LiDAR detections then get matched nearest-neighbor. When a camera projection lands within 1.0 m of a LiDAR cluster the two fuse together, and each sensor contributes what it is actually good at. LiDAR keeps the depth because that measurement is accurate. The camera keeps the color label because that classification has been verified.
The weave is gone
With cones fused and color-labeled, the controller stops making a binary left-or-right choice. It takes the geometric midpoint between the blue and yellow boundary averages and steers proportionally toward it. On straights, where the binary version used to slam between full lock left and full lock right every control cycle, the proportional one settles smoothly to zero. Both controllers run at 20 Hz with the same 4 m/s target speed, so color-aware fusion is the only thing that changed and the only thing that can explain the difference in behavior.
There is a layered fallback so the car keeps steering if the camera drops out. Fused color-aware control when fused cones are available, the binary density heuristic when they are not, and straight ahead as a last resort. Next on the list is a proper quantitative pass covering lap completion rate, RMS lateral tracking error and YOLOv5 recall on held-out frames, then swapping the greedy nearest-neighbor matcher for something that handles partial occlusion. After that, onto a physical car with hardware-synchronized sensors.
This same fused stack is what generates ground truth and demonstrations for the reinforcement learning work, where I tried replacing this hand-coded controller with a learned policy.