...

The UndistortPoints function transforms the image/pixel coordinates to metric coordinates on the driving surface (floor). The calculation is based on a calibration image. See https://git.rwth-aachen.de/CPM/Project/Lab/softwaregithub.com/embedded-software-laboratory/cpm-lab/tree/master/matlab_scripts/camera_calibration

The DetectVehicles function groups and classifies points as front, center, back_left or back_right. Points that appear to not belong to a vehicle are discarded.

...

The PoseCalculation function calculates the vehicle reference pose based on the front, back_left and back_right points. The calculation is based on manually collected calibration data. See https://git.rwth-aachen.de/CPM/Project/Lab/softwaregithub.com/embedded-software-laboratory/cpm-lab/tree/master/matlab_scripts/ips_pose_calibration

The implementation can be found at https://git.rwth-aachen.de/CPM/Project/Lab/softwaregithub.com/embedded-software-laboratory/cpm-lab/tree/master/ips2/src

Calibration

Camera Calibration

...

The Matlab function detectCheckerboardPoints is used to determine a list of matching points (x_floor, y_floor, x_image, y_image). Then a 5th order 2D polynomial is fitted to the data using linear least squares.

See https://git.rwth-aachen.de/CPM/Project/Lab/softwaregithub.com/embedded-software-laboratory/cpm-lab/tree/master/matlab_scripts/camera_calibration

Anchor
pose_calibration pose_calibration

Pose Calibration

...