[MatLab] Stereoskopia - odległość przy zbieżnych kamerach

Witam.

Pracuję nad programem, który wyliczył by odległość od bazy na podstawie położenia w obu kadrach. Mam już zrealizowaną analizę obrazu, wyliczam ile stopni (w poziomie) wynosi odchyłka od osi optycznej przedmiotu w obrazie.

I tak na przykład gdy obiekt znajduje się w trójkącie tworzonym przez osie optyczne i bazę wszystko jest ok wynik nawet bliski rzeczywistemu.
Natomiast gdy obiekt znajduje się z boku poza osią optyczną wszystko się gmatwa i wylicza mi głupoty.

np. kąt alfa jest między linią bazową a linią łączącą matrycę z obiektem
alfaL = 177.1493
alfaP = 86.1427
dla tych wartości powinno wyjść w okolicach 12,1m (odległość)

może ktoś gmatwam z funkcjami trygonometrycznymi dla kątów > 90° ? :/
próbowałem na różne sposoby i metodą prób i błędów - nie działa

baza = 500mm
ogniskowa f = 12mm
kowergencja = 14m
ilość pikseli w poziomie 1280
wielkość piksela 5,2um

Czy mógłby mi ktoś podpowiedzieć jak powinien wyglądać taki uniwersalny algorytm w matlabie, niezależnie od położenia przedmiotu?


Przepraszam za mało czytelny kod, ale cały czas w nim dłubie, dodaje, zmieniam. Jednak bardziej chodzi mi o to jak poradzić sobie z kodem (funkcji tryg. na >90°)

Za szybką, precyzyjną i wyczerpującą odpowiedź dzięki której zacznie śmigać program mogę zaproponować 300pt (chyba można przekazywać?) jeśli komuś się przydadzą lub postawić dużego browara


Dodano po 3 [godziny] 19 [minuty]:

Ok, chyba się już udało.