248 lines
3.5 KiB
TeX
248 lines
3.5 KiB
TeX
\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} |