Phase 1 abgeschlossen: Positionen werden erkannt.

Positionen aus den Merkern heraus erkennbar. Viele Bilder gleichzeitig verarbeitbar.
This commit is contained in:
chk
2026-05-25 22:16:11 +02:00
parent f37097ea96
commit 5a7176920a
11 changed files with 3005 additions and 120 deletions

14
documentation/Phase1.aux Normal file
View File

@@ -0,0 +1,14 @@
\relax
\@writefile{toc}{\contentsline {section}{\numberline {1}Ziel}{1}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {2}Koordinatensysteme}{1}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {3}solvePnP}{1}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {4}Kamerapose}{2}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {5}Markerposition in Weltkoordinaten}{2}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {6}Markerrotation in Weltkoordinaten}{2}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {7}Gewichtung der Beobachtungen}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {8}Gewichtete Positionsfusion}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {9}Rotationsfusion}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {10}Qualitätsmetriken}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {11}Rigid-Body Erweiterung}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {12}Spätere Erweiterungen}{4}{}\protected@file@percent }
\gdef \@abspage@last{4}

BIN
documentation/Phase1.pdf Normal file

Binary file not shown.

248
documentation/Phase1.tex Normal file
View File

@@ -0,0 +1,248 @@
\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}

View File

@@ -0,0 +1,28 @@
\relax
\@writefile{toc}{\contentsline {section}{\numberline {1}Ziel}{1}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {2}Ausgangslage}{1}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {3}Grundidee}{2}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {4}Vorteil}{2}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {5}Erwartete Verbesserungen}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {5.1}Stabilität}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {5.2}Konsistenz}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {5.3}Multi-Camera-Verkettung}{3}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {6}Benötigte Daten}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {6.1}Bereits vorhanden}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsubsection}{\numberline {6.1.1}Absolute Marker}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsubsection}{\numberline {6.1.2}Relative Markerpositionen}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsubsection}{\numberline {6.1.3}Body-Zuordnung}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsubsection}{\numberline {6.1.4}Gelenke}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {7}Noch fehlende Daten}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {7.1}Marker-Orientierung relativ zum Body}{4}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {8}Geplante Solver-Strategie}{5}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {8.1}Phase 2A --- Rigid Body Fit}{5}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {8.2}Phase 2B --- Joint Constraints}{5}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {8.3}Phase 2C --- Global Optimization}{5}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {9}Wichtige Architekturentscheidung}{6}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {10}Geplante Datenstruktur}{6}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {10.1}Weltpose eines Körpers}{6}{}\protected@file@percent }
\@writefile{toc}{\contentsline {subsection}{\numberline {10.2}Relative Markerdefinition}{6}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {11}Phase 1 Reminder}{6}{}\protected@file@percent }
\@writefile{toc}{\contentsline {section}{\numberline {12}Zielbild}{7}{}\protected@file@percent }
\gdef \@abspage@last{7}

Binary file not shown.

View File

@@ -0,0 +1,348 @@
\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}