2025-summer/team-3
12
Fork 0
Code Issues Pull requests Projects Releases Activity

rangelocator: fractional ranges?

Browse source
This commit is contained in:
Laura Klünder 2023-11-11 16:49:12 +01:00
parent 94af40d2f1
commit db07c57c35
1 changed files with 16 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

28
src/c3nav/routing/rangelocator.py
View file

@ -11,7 +11,6 @@ from scipy.optimize import least_squares
from c3nav.mapdata.models import MapUpdate from c3nav.mapdata.models import MapUpdate
from c3nav.mapdata.models.geometry.space import RangingBeacon from c3nav.mapdata.models.geometry.space import RangingBeacon
from c3nav.mapdata.utils.locations import CustomLocation from c3nav.mapdata.utils.locations import CustomLocation
from c3nav.mesh.messages import MeshMessageType
from c3nav.routing.router import Router from c3nav.routing.router import Router
@ -72,9 +71,6 @@ class RangeLocator:
) if i is not None ) if i is not None
) )
# get index of all known beacons
beacons_i = tuple(i for i, peer in ranges)
# create 2d array with x, y, z, distance as rows # create 2d array with x, y, z, distance as rows
np_ranges = np.hstack(( np_ranges = np.hstack((
self.beacon_positions[tuple(i for i, distance in ranges), :], self.beacon_positions[tuple(i for i, distance in ranges), :],
@ -90,17 +86,22 @@ class RangeLocator:
# TODO: three points aren't really enough for precise results? hm. maybe just a 2d fix then? # TODO: three points aren't really enough for precise results? hm. maybe just a 2d fix then?
pass pass
factor = None dimensions = 2
measured_ranges = np_ranges[:, 3]
measured_ranges = measured_ranges / np.max(measured_ranges)
# rating the guess by calculating the distances # rating the guess by calculating the distances
def rate_guess(guess): def rate_guess(guess):
diffs = scipy.linalg.norm(np_ranges[:, :3] - guess[:3], axis=1) * (factor or guess[3]) - np_ranges[:, 3] guessed_ranges = scipy.linalg.norm(np_ranges[:, :dimensions] - guess[:dimensions], axis=1)
#if (diffs < -200).any(): guessed_ranges /= np.max(guessed_ranges)
# return diffs+10000-np.clip(diffs, None, -200)*10 diffs = guessed_ranges-measured_ranges
if (diffs < -200).any():
return diffs+100-np.clip(diffs, None, -200)*10
return diffs return diffs
# initial guess i the average of all beacons, with scale 1 # initial guess i the average of all beacons, with scale 1
initial_guess = np.append(np.average(np_ranges[:, :3], axis=0), 1) initial_guess = np.average(np_ranges[:, :dimensions], axis=0)
# here the magic happens # here the magic happens
results = least_squares(rate_guess, initial_guess) results = least_squares(rate_guess, initial_guess)
@ -117,8 +118,11 @@ class RangeLocator:
) )
print("measured ranges:", ", ".join(("%.2f" % i) for i in tuple(np_ranges[:, 3]))) print("measured ranges:", ", ".join(("%.2f" % i) for i in tuple(np_ranges[:, 3])))
print("result ranges:", ", ".join(("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :3] - results.x[:3], axis=1)*(factor or results.x[3])))) print("result ranges:", ", ".join(
print("height:", results.x[2]) ("%.2f" % i) for i in tuple(scipy.linalg.norm(np_ranges[:, :dimensions] - results.x[:dimensions], axis=1))
print("scale:", (factor or results.x[3])) ))
if dimensions > 2:
print("height:", results.x[2])
# print("scale:", (factor or results.x[3]))
return location return location