\documentclass[a4paper,11pt]{article}

\usepackage[utf8]{inputenc}
\usepackage[T1]{fontenc}
\usepackage{geometry}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{listings}
\usepackage{xcolor}


\title{Phase 2 --- Kinematic-Constrained Multi-Camera Solver}
\author{}
\date{}

\begin{document}

\maketitle

\section{Ziel}

Nach Phase 1 existiert:

\begin{itemize}
    \item ein gemeinsames Weltkoordinatensystem
    \item Kameraposen aller Kameras
    \item bekannte absolute Markerpositionen
    \item fusionierte Beobachtungen mehrerer Kameras
\end{itemize}

Phase 2 erweitert das System um:

\begin{itemize}
    \item Rigid-Body Constraints
    \item mechanische Zusammenhänge
    \item Gelenke
    \item relative Marker-Geometrien (\texttt{relPos})
    \item Stabilisierung bei wenigen sichtbaren Markern
\end{itemize}

\section{Ausgangslage}

Aktuell wird jeder Marker unabhängig behandelt.

Das ist suboptimal, weil:

\begin{itemize}
    \item viele Marker nur kurz sichtbar sind
    \item oft nur 1--2 Marker eines Bauteils sichtbar sind
    \item solvePnP bei wenigen Markern instabil wird
    \item Markerrauschen direkt in die Weltkoordinaten eingeht
\end{itemize}

Mechanisch sind die Marker jedoch nicht unabhängig.

Mehrere Marker gehören jeweils zu:

\begin{itemize}
    \item Arm1
    \item Arm2
    \item Joint1
    \item Base
    \item Finger1
    \item Finger2
\end{itemize}

und bilden jeweils starre Körper (Rigid Bodies).

\section{Grundidee}

Statt einzelne Marker zu lösen:

\begin{verbatim}
Marker -> Welt
\end{verbatim}

wird gelöst:

\begin{verbatim}
RigidBody -> Welt
\end{verbatim}

und daraus:

\begin{verbatim}
Marker = RigidBody * relTransform
\end{verbatim}

\section{Vorteil}

Schon ein einzelner sichtbarer Marker kann:

\begin{itemize}
    \item einen ganzen Körper stabilisieren
    \item andere unsichtbare Marker indirekt bestimmen
\end{itemize}

Beispiel:

Wenn Marker 198 sichtbar ist
und Marker 229 relativ dazu bekannt ist,
dann kann Marker 229 geschätzt werden,
auch wenn er aktuell unsichtbar ist.

\section{Erwartete Verbesserungen}

\subsection{Stabilität}

Deutlich stabilere Pose-Schätzung bei:

\begin{itemize}
    \item Motion Blur
    \item wenigen sichtbaren Markern
    \item schlechten Blickwinkeln
    \item Teilverdeckungen
\end{itemize}

\subsection{Konsistenz}

Marker eines Bauteils bleiben:

\begin{itemize}
    \item geometrisch korrekt
    \item starr
    \item ohne unrealistische Verzerrungen
\end{itemize}

\subsection{Multi-Camera-Verkettung}

Kameras können indirekt gekoppelt werden.

Beispiel:

Cam1 sieht:
\begin{itemize}
    \item Marker 1,2,3
\end{itemize}

Cam2 sieht:
\begin{itemize}
    \item Marker 3,198
\end{itemize}

Cam3 sieht:
\begin{itemize}
    \item Marker 198,229
\end{itemize}

Dadurch wird:
\begin{itemize}
    \item Arm1 relativ zur Welt bestimmbar
    \item obwohl keine einzelne Kamera alles sieht
\end{itemize}

\section{Benötigte Daten}

\subsection{Bereits vorhanden}

\subsubsection{Absolute Marker}

\begin{verbatim}
"position":[x,y,z]
\end{verbatim}

für Board-Marker.

\subsubsection{Relative Markerpositionen}

\begin{verbatim}
"relPos":[x,y,z]
\end{verbatim}

für Marker auf einem Rigid Body.

\subsubsection{Body-Zuordnung}

\begin{verbatim}
"on":"Arm1"
\end{verbatim}

\subsubsection{Gelenke}

\begin{verbatim}
"type":"revolute"
"axis":[1,0,0]
\end{verbatim}

\section{Noch fehlende Daten}

\subsection{Marker-Orientierung relativ zum Body}

Aktuell existiert nur:

\begin{verbatim}
"relPos"
\end{verbatim}

Empfohlen wird zusätzlich:

\begin{verbatim}
"relRot":[rx,ry,rz]
\end{verbatim}

oder alternativ:

\begin{verbatim}
"normal":[x,y,z]
"up":[x,y,z]
\end{verbatim}

Denn Marker besitzen nicht nur Position,
sondern auch Orientierung.

Das verbessert spätere Pose-Fits deutlich.

\section{Geplante Solver-Strategie}

\subsection{Phase 2A --- Rigid Body Fit}

Zunächst:

\begin{itemize}
    \item pro Element einen starren Körper fitten
    \item noch keine Gelenkoptimierung
\end{itemize}

Beispiel:

\begin{verbatim}
T_world_arm1
\end{verbatim}

wird geschätzt.

Alle Marker von Arm1 folgen daraus automatisch.

\subsection{Phase 2B --- Joint Constraints}

Danach:

\begin{itemize}
    \item Gelenkachsen erzwingen
    \item Rotationen einschränken
    \item mechanische Grenzen verwenden
\end{itemize}

Beispiel:

\begin{verbatim}
Arm2 darf sich nur um Y drehen
\end{verbatim}

\subsection{Phase 2C --- Global Optimization}

Später optional:

\begin{itemize}
    \item vollständiges Bundle Adjustment
    \item gleichzeitige Optimierung aller:
    \begin{itemize}
        \item Kameras
        \item Marker
        \item Bodies
        \item Gelenkwinkel
    \end{itemize}
\end{itemize}

\section{Wichtige Architekturentscheidung}

NICHT:

\begin{verbatim}
Marker einzeln lösen
\end{verbatim}

SONDERN:

\begin{verbatim}
Bodies + Constraints lösen
\end{verbatim}

Marker werden damit Beobachtungen,
nicht mehr primäre Zustände.

\section{Geplante Datenstruktur}

\subsection{Weltpose eines Körpers}

\begin{verbatim}
{
  "body":"Arm1",
  "worldPose":{
    "position":[x,y,z],
    "rotationMatrix":[...]
  }
}
\end{verbatim}

\subsection{Relative Markerdefinition}

\begin{verbatim}
{
  "id":198,
  "on":"Arm1",
  "relPos":[x,y,z],
  "relRot":[rx,ry,rz]
}
\end{verbatim}

\section{Phase 1 Reminder}

Phase 1 bleibt weiterhin wichtig:

\begin{itemize}
    \item alle Kameras finden
    \item alle Detection-Dateien laden
    \item gemeinsame Marker fusionieren
    \item Weltkoordinaten berechnen
    \item Qualitätsmetriken speichern
    \item auch schlechte / unvollständige Beobachtungen abspeichern
\end{itemize}

Die Ergebnisse von Phase 1 dienen als Eingang für Phase 2.

\section{Zielbild}

Langfristig entsteht:

\begin{itemize}
    \item ein globales Robotermodell
    \item mit mehreren Kameras
    \item mehreren Rigid Bodies
    \item Gelenken
    \item Unsicherheiten
    \item Qualitätsmetriken
    \item temporaler Stabilisierung
\end{itemize}

basierend auf:

\begin{itemize}
    \item ArUco-Beobachtungen
    \item Mechanik
    \item Kinematik
    \item Multi-View-Geometrie
\end{itemize}

\end{document}