\documentclass[a4paper,11pt]{article}

\usepackage[utf8]{inputenc}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{geometry}
\geometry{margin=2.5cm}

\title{Mathematische Beschreibung von \texttt{3\_fuse\_markers\_world.py}}
\author{}
\date{}

\begin{document}

\maketitle

\section{Ziel}

Das Script fusioniert mehrere ArUco-Detektionen aus mehreren Kameras zu einem gemeinsamen Weltmodell.

Gegeben sind:

\begin{itemize}
\item Kameraposen im Weltkoordinatensystem
\item 2D Marker-Detektionen pro Kamera
\item Kameraintrinsics
\item Markergröße
\end{itemize}

Gesucht sind:

\begin{itemize}
\item Weltposition aller Marker
\item Markerorientierungen
\item Qualitätsmetriken
\end{itemize}

\section{Koordinatensysteme}

Verwendete Systeme:

\begin{itemize}
\item Weltkoordinatensystem $W$
\item Kamerakoordinatensystem $C$
\item Markerkoordinatensystem $M$
\end{itemize}

\section{solvePnP}

Für jeden Marker wird mittels OpenCV solvePnP berechnet:

\[
X_C = R_{CM} X_M + t_{CM}
\]

Dabei gilt:

\begin{itemize}
\item $R_{CM}$ = Rotation Marker $\rightarrow$ Kamera
\item $t_{CM}$ = Translation Marker $\rightarrow$ Kamera
\end{itemize}

\section{Kamerapose}

Aus der vorher berechneten Kamerapose:

\[
X_W = R_{WC} X_C + t_{WC}
\]

mit:

\[
t_{WC} = C_W
\]

(Kameraposition in Weltkoordinaten)

\section{Markerposition in Weltkoordinaten}

Markerzentrum:

\[
p_M =
\begin{bmatrix}
0 \\
0 \\
0
\end{bmatrix}
\]

Markerposition im Kamerasystem:

\[
p_C = R_{CM} p_M + t_{CM}
\]

Da $p_M = 0$:

\[
p_C = t_{CM}
\]

Transformation ins Weltkoordinatensystem:

\[
p_W = R_{WC} p_C + t_{WC}
\]

Somit:

\[
p_W = R_{WC} t_{CM} + t_{WC}
\]

\section{Markerrotation in Weltkoordinaten}

Die Markerrotation relativ zur Kamera:

\[
R_{CM}
\]

Die Kamerarotation relativ zur Welt:

\[
R_{WC}
\]

Markerrotation relativ zur Welt:

\[
R_{WM} = R_{WC} R_{CM}
\]

Dies ist die zentrale Rotationsgleichung des Scripts.

\section{Gewichtung der Beobachtungen}

Mehrere Kameras können denselben Marker beobachten.

Für jede Beobachtung wird ein Gewicht berechnet:

\[
w_i =
w_{\text{confidence}}
\cdot
w_{\text{area}}
\cdot
w_{\text{view}}
\cdot
w_{\text{reprojection}}
\]

Typische Faktoren:

\begin{itemize}
\item Marker Confidence
\item Markergröße in Pixel
\item Sichtwinkel
\item Distanz zum Bildrand
\item Reprojektionsfehler
\end{itemize}

\section{Gewichtete Positionsfusion}

Die endgültige Markerposition:

\[
p =
\frac{
\sum_i w_i p_i
}{
\sum_i w_i
}
\]

Dies entspricht einem gewichteten Mittelwert.

\section{Rotationsfusion}

Rotationen werden gesammelt:

\[
R_1, R_2, ..., R_n
\]

Eine einfache erste Näherung:

\begin{itemize}
\item Eulerwinkel mitteln
\item oder Quaternionen mitteln
\end{itemize}

Später empfohlen:

\begin{itemize}
\item SVD-basierte Rotationsmittelung
\item Lie-Group Mittelung auf $SO(3)$
\end{itemize}

\section{Qualitätsmetriken}

Das Script berechnet:

\begin{itemize}
\item Anzahl beobachtender Kameras
\item Positionsstreuung
\item Reprojektionsfehler
\item Gesamtgewicht
\item Sichtwinkel
\end{itemize}

Beispiel:

\[
\sigma =
\sqrt{
\frac{1}{N}
\sum_i ||p_i - \bar{p}||^2
}
\]

\section{Rigid-Body Erweiterung}

Später können Marker über bekannte Relativpositionen gekoppelt werden.

Beispiel:

\[
p_{M2} = p_{M1} + R_{Body} \Delta p
\]

Dadurch können Marker rekonstruiert werden, selbst wenn sie nicht direkt sichtbar sind.

\section{Spätere Erweiterungen}

Geplant:

\begin{itemize}
\item Bundle Adjustment
\item Kinematic Constraints
\item Joint Solver
\item Graph Optimization
\item Temporal Tracking
\end{itemize}

\end{document}