Back to Repositories

Testing Coordinate System Transformations in OpenPilot

This test suite validates coordinate system transformations in the OpenPilot project, focusing on conversions between ECEF (Earth-Centered, Earth-Fixed), NED (North-East-Down), and geodetic coordinate systems. The tests ensure accurate position calculations and coordinate transformations essential for autonomous driving applications.

Test Coverage Overview

The test suite provides comprehensive coverage of coordinate transformation functions:

Small distance calculations in local coordinates
ECEF to geodetic conversions and vice versa
NED coordinate transformations with batch processing
Validation of saved reference results

Implementation Analysis

The implementation uses NumPy for efficient array operations and mathematical calculations. The testing approach employs both single-point and batch conversion tests, with careful attention to numerical precision using relative and absolute tolerance checks.

Technical Details

Testing tools and configuration:

NumPy testing framework (np.testing)
Assert methods for array comparison
Relative tolerance of 1e-9
Absolute tolerance ranging from 1e-4 to 1e-7
Test data includes various global coordinates

Best Practices Demonstrated

The test suite demonstrates several testing best practices:

Comprehensive edge case coverage
Precise numerical comparisons
Modular test organization
Use of pre-computed reference values
Both single and batch processing validation

commaai/openpilot

common/transformations/tests/test_coordinates.py

            
import numpy as np

import openpilot.common.transformations.coordinates as coord

geodetic_positions = np.array([[37.7610403, -122.4778699, 115],
                                 [27.4840915, -68.5867592, 2380],
                                 [32.4916858, -113.652821, -6],
                                 [15.1392514, 103.6976037, 24],
                                 [24.2302229, 44.2835412, 1650]])

ecef_positions = np.array([[-2711076.55270557, -4259167.14692758,  3884579.87669935],
                          [ 2068042.69652729, -5273435.40316622,  2927004.89190746],
                          [-2160412.60461669, -4932588.89873832,  3406542.29652851],
                          [-1458247.92550567,  5983060.87496612,  1654984.6099885 ],
                          [ 4167239.10867871,  4064301.90363223,  2602234.6065749 ]])

ecef_positions_offset = np.array([[-2711004.46961115, -4259099.33540613,  3884605.16002147],
                                  [ 2068074.30639499, -5273413.78835412,  2927012.48741131],
                                  [-2160344.53748176, -4932586.20092211,  3406636.2962545 ],
                                  [-1458211.98517094,  5983151.11161276,  1655077.02698447],
                                  [ 4167271.20055269,  4064398.22619263,  2602238.95265847]])


ned_offsets = np.array([[78.722153649976391, 24.396208657446344, 60.343017506838436],
                       [10.699003365155221, 37.319278617604269, 4.1084100025050407],
                       [95.282646251726959, 61.266689955574428, -25.376506058505054],
                       [68.535769283630003, -56.285970011848889, -100.54840137956515],
                       [-33.066609321880179, 46.549821994306861, -84.062540548335591]])

ecef_init_batch = np.array([2068042.69652729, -5273435.40316622,  2927004.89190746])
ecef_positions_offset_batch = np.array([[ 2068089.41454771, -5273434.46829148,  2927074.04783672],
                                        [ 2068103.31628647, -5273393.92275431,  2927102.08725987],
                                        [ 2068108.49939636, -5273359.27047121,  2927045.07091581],
                                        [ 2068075.12395611, -5273381.69432566,  2927041.08207992],
                                        [ 2068060.72033399, -5273430.6061505,  2927094.54928305]])

ned_offsets_batch = np.array([[  53.88103168,   43.83445935,  -46.27488057],
                              [  93.83378995,   71.57943024,  -30.23113187],
                              [  57.26725796,   89.05602684,   23.02265814],
                              [  49.71775195,   49.79767572,   17.15351015],
                              [  78.56272609,   18.53100158,  -43.25290759]])


class TestNED:
  def test_small_distances(self):
    start_geodetic = np.array([33.8042184, -117.888593, 0.0])
    local_coord = coord.LocalCoord.from_geodetic(start_geodetic)

    start_ned = local_coord.geodetic2ned(start_geodetic)
    np.testing.assert_array_equal(start_ned, np.zeros(3,))

    west_geodetic = start_geodetic + [0, -0.0005, 0]
    west_ned = local_coord.geodetic2ned(west_geodetic)
    assert np.abs(west_ned[0]) < 1e-3
    assert west_ned[1] < 0

    southwest_geodetic = start_geodetic + [-0.0005, -0.002, 0]
    southwest_ned = local_coord.geodetic2ned(southwest_geodetic)
    assert southwest_ned[0] < 0
    assert southwest_ned[1] < 0

  def test_ecef_geodetic(self):
    # testing single
    np.testing.assert_allclose(ecef_positions[0], coord.geodetic2ecef(geodetic_positions[0]), rtol=1e-9)
    np.testing.assert_allclose(geodetic_positions[0, :2], coord.ecef2geodetic(ecef_positions[0])[:2], rtol=1e-9)
    np.testing.assert_allclose(geodetic_positions[0, 2], coord.ecef2geodetic(ecef_positions[0])[2], rtol=1e-9, atol=1e-4)

    np.testing.assert_allclose(geodetic_positions[:, :2], coord.ecef2geodetic(ecef_positions)[:, :2], rtol=1e-9)
    np.testing.assert_allclose(geodetic_positions[:, 2], coord.ecef2geodetic(ecef_positions)[:, 2], rtol=1e-9, atol=1e-4)
    np.testing.assert_allclose(ecef_positions, coord.geodetic2ecef(geodetic_positions), rtol=1e-9)


  def test_ned(self):
    for ecef_pos in ecef_positions:
      converter = coord.LocalCoord.from_ecef(ecef_pos)
      ecef_pos_moved = ecef_pos + [25, -25, 25]
      ecef_pos_moved_double_converted = converter.ned2ecef(converter.ecef2ned(ecef_pos_moved))
      np.testing.assert_allclose(ecef_pos_moved, ecef_pos_moved_double_converted, rtol=1e-9)

    for geo_pos in geodetic_positions:
      converter = coord.LocalCoord.from_geodetic(geo_pos)
      geo_pos_moved = geo_pos + np.array([0, 0, 10])
      geo_pos_double_converted_moved = converter.ned2geodetic(converter.geodetic2ned(geo_pos) + np.array([0, 0, -10]))
      np.testing.assert_allclose(geo_pos_moved[:2], geo_pos_double_converted_moved[:2], rtol=1e-9, atol=1e-6)
      np.testing.assert_allclose(geo_pos_moved[2], geo_pos_double_converted_moved[2], rtol=1e-9, atol=1e-4)

  def test_ned_saved_results(self):
    for i, ecef_pos in enumerate(ecef_positions):
      converter = coord.LocalCoord.from_ecef(ecef_pos)
      np.testing.assert_allclose(converter.ned2ecef(ned_offsets[i]),
                                 ecef_positions_offset[i],
                                 rtol=1e-9, atol=1e-4)
      np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset[i]),
                                 ned_offsets[i],
                                 rtol=1e-9, atol=1e-4)

  def test_ned_batch(self):
    converter = coord.LocalCoord.from_ecef(ecef_init_batch)
    np.testing.assert_allclose(converter.ecef2ned(ecef_positions_offset_batch),
                                                           ned_offsets_batch,
                                                           rtol=1e-9, atol=1e-7)
    np.testing.assert_allclose(converter.ned2ecef(ned_offsets_batch),
                                                           ecef_positions_offset_batch,
                                                           rtol=1e-9, atol=1e-7)