Camera localisation on Drive PX: the corner that wasn't flat

The car lost its position at one corner while its detections looked fine. A debugger I built in a week showed why: the road tilted, and the projection assumed it didn't. The fix was the vehicle's IMU.

In 2018 our test car localised itself by matching what its camera saw against an HD map. A segmentation network found curbs and an edge network found lines; both were projected onto the ground and scored against the map’s curbs, and a particle filter or Kalman fit turned the best match into a pose. Odometry carried the pose between camera updates. It all ran on a Drive PX in the car, and I joined the integration in June 2018.

A test drive on the ESR Labs campus, Balanstraße, Munich, May 2018, with the localisation overlay: detected curbs projected onto the road and the map view top right.

A miss at one corner

At one corner the localisation kept picking the wrong pose. The detections were good: the curbs were found where they should be. The map was right. Yet the score preferred a wrong position, and nothing in the logged numbers said why.

A debugger to see the score

So in July 2018 I spent a week on a visual debugger. I re-implemented the score in plain Python on purpose, so every term could be inspected: the weighted mean squared distance from projected curb pixels to the map’s curbs, with a linear cut-off beyond 10 pixels so outliers can’t dominate.

The debugger draws the map, the candidate poses shaded by score, the best pose with its projected curbs on top of the map, and a heat map of the score. Candidates sit on a grid aligned with the car’s heading, so rows and columns of the score read directly as along-track and cross-track error.

The debugger’s candidate grid: 4 m across, 7 m along, 1 m spacing, aligned with the car’s heading.

The road wasn’t flat

With the projected curbs drawn over the map, the corner explained itself. The curbs were detected in the right place in the image, but they landed in the wrong place on the ground. The corner wasn’t flat, so the car tilted as it drove through it.

Projecting a pixel onto the ground needs the camera’s height and angle to the road. The pipeline took those from the calibration and assumed a flat road, so tilt didn’t exist for it. When the car tilted, the same pixel’s ray met the ground somewhere else, every projected curb shifted, and the score rewarded the pose that matched the shifted curbs.

Schematic. A tilt of the car changes where each camera ray meets the road, so curbs that were detected correctly are placed wrongly on the map.

The fix: the IMU

Odometry was already in the loop, but it only tells you how far and in which direction the car moved on a plane. So we added the vehicle’s IMU as an input next to the odometry. Its roll and pitch now enter the camera-to-ground projection, so the curbs are projected with the car’s actual tilt instead of a flat-road assumption.

Make the failure visible before you tune anything. No change to the networks or the score would have fixed this one.

What else the debugger caught

Two silent bugs: particles were projected from the car’s origin instead of the camera, and the distance map’s axes were transposed.
Straight roads are ambiguous along the track. Across the road the curbs pin the pose down; along a straight road every position looks the same and the score is flat. That needs odometry or other cues, not a better detector.

The system around it

On the car: a generic TensorFlow executor in C++/CUDA, one per GPU, configured from JSON, so a new network is a config change. Segmentation and edge detection ran as separate components, and a car-tracker component wrapped the localisation in three modes: odometry, particle filter and Kalman fit.
In 2019, a C++ Kalman library for ego-vehicle localisation: a position filter and a quaternion EKF paired into one 6-DoF tracker, with partial filters so GPS and IMU updates don’t leak one sensor’s errors into another’s estimate.