[Python] Finding the coordinates of a point in 3D space

I'm looking for a point in 3D space using three spheres

To find a point, I use a system of three equations of spheres (I know the coordinates of the points of the spheres and their radii)

If the desired point is located in z at a height higher than or equal to the center of the point, then the desired point is searched normally

But if it’s lower, then the z coordinate is being searched incorrectly

import numpy as np
from scipy.optimize import least_squares

def GetTag3DCoordinates(anchors, radii):

# Сортировка радиусов и сфер по id
anchors = sorted(anchors, key=lambda x: x[0])
radii = sorted(radii, key=lambda x: x[0])

# Извлечение второго значения радиуса для расчетов
radii = [r[1] for r in radii]

def residuals(vars, anchors, radii):
x, y, z = vars
debug = [np.sqrt((x - ax)**2 + (y - ay)**2 + (z - az)**2) - r
for (_, ax, ay, az), r in zip(anchors, radii)]
return debug

# Сортировка радиусов и выбор трех минимальных
sorted_radii = np.argsort(radii)
selected_indices = sorted_radii[:3]

selected_anchors = [anchors for i in selected_indices]
selected_radii = [radii for i in selected_indices]

# Начальное приближение для координат метки (центр якорей)
x0 = np.mean([a[1] for a in selected_anchors])
y0 = np.mean([a[2] for a in selected_anchors])
z0 = np.mean([a[3] for a in selected_anchors])

initial_guess = [x0, y0, z0]

try:
# Оптимизация методом наименьших квадратов с использованием всех якорей
result1 = least_squares(residuals, initial_guess, args=(selected_anchors, selected_radii))

final_result1 = np.array([result1.x[0], result1.x[1], result1.x[2]])

# Возвращаем оба результата
return final_result1
except Exception as e:
print(f"Error occurred: {e}")
return np.array([-1, -1, -1]), np.array([-1, -1, -1])

# Пример использования функции
if __name__ == "__main__":
anchors = [
(1, 222.8, 125.4, 69.4),
(2, 222.8, 191.2, 69.4),
(3, 347.9, 125.6, 69.4),
(4, 347.9, 191.2, 69.4)
]

radii = [
(1, 164.4),
(2, 109.6),
(3, 221.4),
(4, 184.4)
]

tag_coords1 = GetTag3DCoordinates(anchors, radii)
print("Координаты метки 1: x={:.2f}, y={:.2f}, z={:.2f}".format(*tag_coords1))



I don’t know exactly where the error is, but I have an assumption that the error may be in the least squares method

Continue reading...