mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-20 13:43:57 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
cfe0ae9b8a4585dc6dbca6e6cdcdbbd2908e7ebf
dragonpilot/selfdrive/mapd/test/test_geo.py
dragonpilot cfe0ae9b8a dragonpilot beta3 
date: 2023-10-09T10:55:55
commit: 91b6e3aecd7170f24bccacb10c515ec281c30295
2023-10-09 10:56:13 -07:00

235 lines
9.7 KiB
Python
Raw Blame History

import unittest
from openpilot.selfdrive.mapd.lib.geo import vectors, ref_vectors, bearing_to_points, distance_to_points
import numpy as np
from numpy.testing import assert_array_almost_equal
from openpilot.selfdrive.mapd.test.mock_data import mockNodesData01
class TestMapsdGeoLibrary(unittest.TestCase):
def test_vectors(self):
points = mockNodesData01.radians
expected = np.array([
[-1.34011951e-05, 1.00776468e-05],
[-5.83610920e-06, 4.41046897e-06],
[-7.83348567e-06, 5.94114032e-06],
[-7.08560788e-06, 5.30408795e-06],
[-6.57632550e-06, 4.05791838e-06],
[-1.16077872e-06, 6.91151252e-07],
[-1.53178098e-05, 9.62215139e-06],
[-5.76314175e-06, 3.55176643e-06],
[-1.61124141e-05, 9.86127759e-06],
[-1.48006628e-05, 8.58192512e-06],
[-1.72237209e-06, 1.60570482e-06],
[-8.68985228e-06, 9.22062311e-06],
[-1.42922812e-06, 1.51494711e-06],
[-3.39761486e-06, 2.57087743e-06],
[-2.75467373e-06, 1.28631255e-06],
[-1.57501989e-05, 5.72309451e-06],
[-2.52143954e-06, 1.34565295e-06],
[-1.65278643e-06, 1.28630942e-06],
[-2.22196114e-05, 1.64360838e-05],
[-5.88675934e-06, 4.08234746e-06],
[-1.83673390e-06, 1.46782408e-06],
[-1.55004206e-06, 1.51843800e-06],
[-1.20451533e-06, 2.06298011e-06],
[-1.91801338e-06, 4.64083285e-06],
[-2.38653483e-06, 5.60076524e-06],
[-1.65269781e-06, 5.78402290e-06],
[-3.66908309e-07, 2.75412965e-06],
[0.00000000e+00, 1.92858882e-06],
[9.09242615e-08, 2.66162711e-06],
[3.14490354e-07, 1.53065382e-06],
[8.66452477e-08, 4.83456208e-07],
[2.41750593e-07, 1.10828411e-06],
[7.43745228e-06, 1.27618831e-05],
[5.59968054e-06, 9.63947367e-06],
[2.01951467e-06, 2.75413219e-06],
[4.59952643e-07, 6.42281301e-07],
[1.74353749e-06, 1.74533121e-06],
[2.57144338e-06, 2.11185266e-06],
[1.46893187e-05, 1.11999169e-05],
[3.84659229e-05, 2.85527952e-05],
[2.71627936e-05, 1.98727946e-05],
[8.44632540e-06, 6.15058628e-06],
[2.29420323e-06, 1.92859222e-06],
[2.58083439e-06, 3.16952222e-06],
[3.76373643e-06, 5.14174911e-06],
[5.32416098e-06, 6.51707770e-06],
[8.62890928e-06, 1.11998258e-05],
[1.25762497e-05, 1.65231340e-05],
[8.90452991e-06, 1.10148240e-05],
[4.86505726e-06, 4.59023120e-06],
[3.85545276e-06, 3.39642031e-06],
[3.48753893e-06, 3.30566145e-06],
[2.99557303e-06, 2.61276368e-06],
[2.15496788e-06, 1.87797727e-06],
[4.10564937e-06, 3.58142649e-06],
[1.53680853e-06, 1.33866906e-06],
[4.99540175e-06, 4.35635790e-06],
[1.37744970e-06, 1.19380643e-06],
[1.74319821e-06, 1.28456429e-06],
[9.99931238e-07, 1.14493663e-06],
[6.42735560e-07, 1.19380547e-06],
[3.66818436e-07, 1.46782199e-06],
[5.45413874e-08, 1.83783170e-06],
[-1.35818548e-07, 1.14842666e-06],
[-5.50758101e-07, 3.02989178e-06],
[-4.58785270e-07, 2.66162724e-06],
[-2.51315555e-07, 1.19031459e-06],
[-3.91409773e-07, 1.65457223e-06],
[-2.14525206e-06, 5.67755902e-06],
[-4.24558096e-07, 1.39102753e-06],
[-1.46936730e-06, 5.32325561e-06],
[-1.37632061e-06, 4.59021715e-06],
[-8.26642899e-07, 4.68097349e-06],
[-6.42702724e-07, 4.95673534e-06],
[-3.66796960e-07, 7.25009780e-06],
[-1.82861669e-07, 8.99542699e-06],
[4.09564134e-07, 6.11214315e-06],
[7.80629912e-08, 1.45734993e-06],
[4.81205526e-07, 7.56076647e-06],
[2.01036346e-07, 2.42775302e-06]])
v = vectors(points)
assert_array_almost_equal(v, expected)
def test_ref_vectors(self):
points = mockNodesData01.radians
expected = np.array([
[1.59924145e-04, -1.07153714e-04],
[1.46520873e-04, -9.70788297e-05],
[1.40683931e-04, -9.26694631e-05],
[1.32849368e-04, -8.67297434e-05],
[1.25762852e-04, -8.14268689e-05],
[1.19185869e-04, -7.73700167e-05],
[1.18024984e-04, -7.66790438e-05],
[1.02705711e-04, -6.70592230e-05],
[9.69420991e-05, -6.35082196e-05],
[8.08284530e-05, -5.36489556e-05],
[6.60268961e-05, -4.50685727e-05],
[6.43043874e-05, -4.34630144e-05],
[5.56137708e-05, -3.42431117e-05],
[5.41844341e-05, -3.27282671e-05],
[5.07866397e-05, -3.01576270e-05],
[4.80318817e-05, -2.88714948e-05],
[3.22813286e-05, -2.31493755e-05],
[2.97598330e-05, -2.18038275e-05],
[2.81069973e-05, -2.05175815e-05],
[5.88679032e-06, -4.08230278e-06],
[0.00000000e+00, 0.00000000e+00],
[-1.83673390e-06, 1.46782408e-06],
[-3.38677236e-06, 2.98626574e-06],
[-4.59127869e-06, 5.04925111e-06],
[-6.50926460e-06, 9.69009532e-06],
[-8.89575243e-06, 1.52908806e-05],
[-1.05483839e-05, 2.10749224e-05],
[-1.09152548e-05, 2.38290571e-05],
[-1.09152276e-05, 2.57576459e-05],
[-1.08242659e-05, 2.84192717e-05],
[-1.05097542e-05, 2.99499212e-05],
[-1.04231024e-05, 3.04333762e-05],
[-1.01813369e-05, 3.15416571e-05],
[-2.74371711e-06, 4.43034426e-05],
[2.85599752e-06, 5.39428964e-05],
[4.87550206e-06, 5.66970360e-05],
[5.33545066e-06, 5.73393202e-05],
[7.07897615e-06, 5.90846634e-05],
[9.65040026e-06, 6.11965396e-05],
[2.43395796e-05, 7.23966392e-05],
[6.28046063e-05, 1.00950641e-04],
[8.99657904e-05, 1.20825635e-04],
[9.84114021e-05, 1.26977201e-04],
[1.00705361e-04, 1.28906084e-04],
[1.03285783e-04, 1.32075942e-04],
[1.07048835e-04, 1.37218192e-04],
[1.12372096e-04, 1.43736004e-04],
[1.20999382e-04, 1.54937080e-04],
[1.33573053e-04, 1.71462176e-04],
[1.42475686e-04, 1.82478533e-04],
[1.47339899e-04, 1.87069658e-04],
[1.51194707e-04, 1.90466811e-04],
[1.54681601e-04, 1.93773152e-04],
[1.57676653e-04, 1.96386513e-04],
[1.59831239e-04, 1.98264929e-04],
[1.63936150e-04, 2.01847201e-04],
[1.65472675e-04, 2.03186195e-04],
[1.70467147e-04, 2.07543619e-04],
[1.71844334e-04, 2.08737728e-04],
[1.73587247e-04, 2.10022678e-04],
[1.74586922e-04, 2.11167839e-04],
[1.75229389e-04, 2.12361789e-04],
[1.75595876e-04, 2.13829694e-04],
[1.75650001e-04, 2.15667538e-04],
[1.75513922e-04, 2.16815933e-04],
[1.74962478e-04, 2.19845700e-04],
[1.74503092e-04, 2.22507224e-04],
[1.74251509e-04, 2.23697482e-04],
[1.73859727e-04, 2.25351966e-04],
[1.71713202e-04, 2.31029044e-04],
[1.71288336e-04, 2.32419977e-04],
[1.69817793e-04, 2.37742908e-04],
[1.68440467e-04, 2.42332824e-04],
[1.67612807e-04, 2.47013617e-04],
[1.66969033e-04, 2.51970213e-04],
[1.66600674e-04, 2.59220232e-04],
[1.66415880e-04, 2.68215619e-04],
[1.66824132e-04, 2.74327850e-04],
[1.66901881e-04, 2.75785216e-04],
[1.67381459e-04, 2.83346086e-04],
[1.67581971e-04, 2.85773882e-04]])
v = ref_vectors(points[20], points)
assert_array_almost_equal(v, expected)
def test_bearing_to_points(self):
points = mockNodesData01.radians
expected = np.array([
2.16112265, 2.15595027, 2.15326799, 2.14916735, 2.14538642,
2.14657678, 2.14694997, 2.1492257, 2.1507589, 2.15676899,
2.16973441, 2.1651606, 2.12270237, 2.11416356, 2.10665211,
2.11201708, 2.19291574, 2.2031069, 2.20136186, 2.17712517,
0., -0.8965745, -0.84815954, -0.73792895, -0.59150953,
-0.5269061, -0.46406215, -0.42954043, -0.4008254, -0.36391371,
-0.33748609, -0.32996807, -0.31223189, -0.06185112, 0.05289544,
0.08578116, 0.0927833, 0.11924233, 0.15640718, 0.32432622,
0.55653415, 0.64003094, 0.6593301, 0.66319086, 0.66367982,
0.66251077, 0.66354137, 0.66302176, 0.66181884, 0.66291139,
0.66714676, 0.67095594, 0.67367984, 0.6765003, 0.67847961,
0.68212344, 0.68345356, 0.68762778, 0.68876073, 0.69070183,
0.69085143, 0.68988665, 0.68753177, 0.68348884, 0.68051081,
0.67220053, 0.66506824, 0.66177969, 0.65712162, 0.63916951,
0.6351146, 0.62025347, 0.60741567, 0.59618923, 0.58521935,
0.57122582, 0.55532475, 0.54636839, 0.54422542, 0.53357655,
0.53037033])
v = bearing_to_points(points[20], points)
assert_array_almost_equal(v, expected)
def test_distance_to_points(self):
points = mockNodesData01.radians
expected = np.array([
1226.82569068, 1120.13820773, 1073.61121415, 1011.10016574,
954.81557436, 905.58045038, 896.97734399, 781.7102819,
738.58271117, 618.26145463, 509.47052142, 494.6403804,
416.22483123, 403.42108699, 376.42615499, 357.15106681,
253.15957483, 235.11572972, 221.77439728, 45.65465979,
0., 14.98414, 28.77606056, 43.49299446,
74.39463425, 112.74005248, 150.19482607, 167.03665191,
178.28443483, 193.80834084, 202.28154097, 205.01173833,
211.22777104, 282.88676739, 344.25957352, 362.66370657,
367.00206795, 379.23951996, 394.82505328, 486.76073331,
757.70254732, 960.03439155, 1023.81434529, 1042.49401713,
1068.53770096, 1109.12696535, 1162.74555108, 1252.847351,
1385.17179405, 1475.42502599, 1517.57849916, 1549.79838056,
1580.12405964, 1605.05483058, 1622.98937809, 1657.19268821,
1669.99157205, 1711.63883132, 1723.09133393, 1736.47655688,
1746.16073119, 1754.63481838, 1763.34186103, 1772.62691273,
1777.76189094, 1790.62024447, 1802.11488235, 1807.1040605,
1813.90756815, 1834.49265566, 1840.00708445, 1861.96087374,
1880.81678093, 1902.42091191, 1926.37194131, 1963.78301115,
2011.62679077, 2046.18028824, 2054.37811294, 2097.30347724,
2111.28586072])
v = distance_to_points(points[20], points)
assert_array_almost_equal(v, expected)
Reference in New Issue View Git Blame Copy Permalink