RealOrangeOne / zoloto

A fiducial marker system powered by OpenCV - Supports ArUco and April
https://zoloto.readthedocs.io/en/stable/
BSD 3-Clause "New" or "Revised" License
13 stars 7 forks source link

Construction of Spherical leads to unknowable position differences #300

Open PeterJCLaw opened 1 year ago

PeterJCLaw commented 1 year ago

Currently the construction of Spherical in BaseMarker.spherical doesn't match up with the conventions of spherical coordinates and leads to some positions being impossible to differentiate.

The implementation currently computes for each axis the angle to projection of the target point onto the plane orthogonal to the axis that passes through the origin. You can also think of this like sweeping a plane around the axis and stopping when it meets the point.

This differs from the convention in that the two axes' angles are both measured independently, rather than one of the angles being measured in the plane described by the other angle.

The most obvious example is to consider what happens to a point directly to the side of the observer (say 1m left, 1m up). In the implemented scheme this will register as 90° on one angular axis and 90° on the other. Changing the height off the ground of the point ends up having no impact on the reported second angle (except when it's at zero height):

https://github.com/RealOrangeOne/zoloto/blob/8b66100fe8d3d617d37a983c6f1460f4250a8da9/zoloto/marker.py#L66-L70

In [3]: from math import atan2, degrees

In [8]: x, y, z = (1, 1, 0)
   ...: degrees(atan2(y, z)), degrees(atan2(x, z))
Out[8]: (90.0, 90.0)

In [9]: x, y, z = (1, 2, 0)
   ...: degrees(atan2(y, z)), degrees(atan2(x, z))
Out[9]: (90.0, 90.0)

libkoki did approximately this (this code actually from the competition-simulator's reimplantation of the same system):

    length = ...
    rot_y = math.atan2(x, z)
    rot_x = math.asin(y / length)

    return Spherical(
        rot_y=rot_y,
        rot_x=rot_x,
        dist=length,
    )

though I'm not sure if that matches the usual conventions either.

The point is that the two angles need computing in different ways.