1. Anuncie Aqui ! Entre em contato fdantas@4each.com.br

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

Discussão em 'Python' iniciado por Stack, Setembro 13, 2024.

  1. Stack

    Stack Membro Participativo

    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...

Compartilhe esta Página