Our perception stack works by combining 3D position detections of the cones with the color detection from the camera.

Here is some raw data after perception and before EKF, with cones plotted where they were detected in the world frame.

I made a custom midline regression program that plots a circle with the radius of the midline.

I made an EKF and cone ID assigner, the data is the same as the gif above, but this time the cones are matched and correlated between frames.

Cone covariances are represented by the size of the dot.

Heres some data for the skidpad track, the green dots are ground truth cone locations, set beforehand, and the purple dots are the detected cones. I wrote an ICP initial pose aligner to align the measured starting pose of the car to the ground truth starting pose.

Collecting skidpad data for mapping testing

Collecting autocross data for mapping testing